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ABSTRACT 



The role of the Maneuvering Target Statistical Tracker (MTST), a Kalman filter tracking 
algorithm based on the Integrated Omstein-Uhlenbeck (lOU) motion process, in the 
Antisubmarine Warfare System Evaluation Tool (ASSET) is examined and its operation 
described. ASSET is a campaign simulation which models open-ocean ASW scenarios featuring 
prosecution of hostile submarines by friendly submarines and aircraft based on cues provided 
by data fusion centers. The heart of each data fusion center is an MTST which integrates new 
contact information into tracks. Comparing the level of sophistication of the tracking algorithm 
to that of the contact data provided to it, a number of simplifications are proposed. These 
include using reduced complexity lOU prediction and Kalman filter equations; the use of pre- 
processed variance data together with the true position of targets to estimate, rather than 
explicitly calculate, updated track states; and limiting contact processing based on information 
content. Results indicate a good simulation of tracker output is produced using a greatly 
simplified algorithm. This technique can be generalized to other types of simulations involving 
target tracking. 
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I. AN INTRODUCTION TO ASSET 



The Antisubmarine Warfare Systems Evaluation Tool (ASSET) is a campaign-level 
simulation which models open ocean scenarios involving submarines, maritime patrol 
aircraft (MPA), shore-based command and data-fusion centers and a wide variety of 
passive acoustic sensors. Active acoustic and non-acoustic sensors are only modeled 
using a simple area sensor model which has a fixed probability of detection and false 
alarm rate. The simulation scenario is specified by a user-supplied "architecture" which 
determines all facets of environment, command control, sensor interaction and platform 
maneuver. This structure is input to a desktop computer/workstation (the Apple 
Macintosh II in this version) through a series of user-friendly windows, each dealing with 
a specific topic. A particular configuration of sensors, data fusion centers, 
communication nodes and tactical platforms interact in a particular geographic location 
against an analogous enemy force structure. 

The scenario is repeated as a Monte Carlo simulation to produce statistically 
meaningful measures of effectiveness (MOE’s). Ouq)ut data regarding the detection, 
localization and prosecution of enemy submarines can then provide a quantitative basis 
for decisions regarding ASW Master Plans, Top Level Warfare Requirements, and a 
variety of emerging technology assessments and system appraisals [Ref. l:p. iv]. These 
results can also be useful in conducting quantitative experimentation with regional force 
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levels, force compositions and commitment strategies; command, control, 
communications and intelligence (C^I) networks; implications of foreign technology 
advances; and fleet exercise planning. The current version of ASSET (version 1.0) limits 
the scope of these scenarios to open ocean search and prosecution of hostile submarines 
by cueing friendly submarines and MPA with wide-area sensors, and modeling the 
supporting C^I networks. The modular nature of the object-oriented structure of ASSET 
makes expanding the scope of the simulation possible [Ref. 2:p. 1-2]. As the 
construction and operating costs of naval platforms spiral upwards, a simulation tool of 
this kind will be increasingly important to intelligently manage resources vis-a-vis a 
rapidly evolving spectrum of possible threats. 

A. OBJECTIVES AND RATIONALE 

The purpose of this thesis is three-fold: 

• to provide a description of the target tracking algorithm used by ASSET and 
how it relates to: 

1. real-world tracking. 

2. the ASSET simulation as a whole. 

• to examine the mathematical development of the tracking algorithm. 

• to examine possible modifications to the existing Kalman Alter tracking 
algorithm to better match the data input to it and increase computational 
efficiency. 
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The use of full Kalman filter equations for elliptical areas of uncertainty 
(AOU’s) is not necessary given the circular AOU’s that ASSET’S sensor model 
produces. Making use of the fact that the computer knows the ground truth locations 
of the platforms, and the limited data input to the filter, it will be shown that it is 
possible to approximate appropriately distributed filtered state positions using 
simplified forms of the Kalman filter matrix computations. The situations when the 
tracker algorithm is used can also be limited based on the information content of the 
contact. Together, these modifications yield a significant reduction in the 
computational complexity of the tracking algorithm, which is the principle goal of this 
thesis. 

The automatic correlator tracker (ACT) implemented in ASSET is a stand-alone 
module based on the Ocean Surveillance Information System (OSIS) baseline upgrade 
single hypothesis, multiple target, Kalman filter-based, correlation and tracking 
algorithm [Ref. 3:p. 4]. This set of routines contains a complete representation of the 
OSIS baseline upgrade automatic correlator tracker’s (OBU-ACT) functions, with the 
exception of LINK, the utility that evaluates track sets which are potentially legs of a 
single target’s track; and EQUATE, which makes track associations for contacts pre- 
correlated to a particular platform based on acoustic or electromagnetic signature. 

The OBU-ACT package is designed to process contact information derived from 
position-only, bearing-only, and position and velocity reports. The covariance 
matrices associated with these reports can be interpreted as an elliptical representation 
of the error in position and velocity. The capability to process line of bearing 
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contacts is not available in ASSET (1.0). The method used to model sensor systems 
in ASSET 1.0 approximates the elliptical errors with circular ones. This reduces the 
complexity of the covariance matrices considerably. Taking advantage of the large 
number of zero elements and repeated values in the matrices actually constructed by 
ASSET, the matrix equations involved in the tracking algorithm can be reduced to 
equivalent scalar ones by eliminating all the zero-multiplied terms. 

Central to this algorithm is the Integrated Omstein-Uhlenbeck (lOU) process 
which models target motion in the Maneuvering Target Statistical Tracker (MTST). 
The MTST uses this model, together with contact data corrupted by noise, to estimate 
the size and position of the Submarine Probability Area (SPA) which has an 86 
percent probability of containing the target’s true position. The major modification to 
the algorithm proposed is based on assuming that the lOU prediction position 
distribution, in like fashion to the contact position distribution, is centered on the 
target’s true position. This results in a distribution of SPA centers that is centered on 
the targets true position also. Thus, only the SPA variance is calculated, not its 
center position, which is drawn randomly from the estimated distribution about the 
true target position. Since contact data does not play a role in computing these 
variances, they may be calculated prior to the start of the simulation. A table of 
variances for the sensor AOU values defined for the scenario and an appropriate 
range of contact rqx)rt interarrival times can be constructed and referred to as needed 
during simulation execution to minimize the time spent processing contact data. 
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B. FUNDAMENTALS OF OPERATION. 



The basic structure of ASSET was adapted from COAST, the Common LISP 
Architectural Study Tool. Common LISP is an object-oriented programming language 
used for rapid prototyping and artificial intelligence work. A general overview of this 
structure is presented to facilitate understanding of how the COAST/ASSET system is 
organized and the critical role of the tracker to simulation operation. 

1. Objects and Object Interactions. 

Object-oriented programs combine data with associated procedures to form a 
hierarchical network of self-contained modules known as objects. These objects can 
then be invoked by each other according to the program methodology. Program 
objects are independent and can be modified without affecting other objects they 
communicate with. The data that is communicated will change, but the relationship 
between the objects does not necessarily have to. Data can be evaluated internally by 
an object without affecting the analogous data in a separate object. 

The principle of instancing allows a single object definition to create any 
number of structures within the program, all obeying the same definition. This 
allows a single definition of a submarine object, for example, to be given many 
different sets of parameters each representing a different class of submarine. Other 
objects may be designated to impart their characteristics to the new submarine object. 
This process of inheritance allows the properties of another object, an acoustic sensor 
object for example, to be created separately from the submarine object. The 
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submarine object can then be designated to be a kind of acoustic sensor and it will 
inherit the properties of the acoustic sensor object in addition to its own. Another 
object such as a SURTASS towed-array surveillance ship object can also inherit the 
properties of the acoustic sensor object, with different parameters, without affecting 
the parameters of the acoustic sensor inherited by the submarine object. 

Complex classes of platforms can be formed by multiple inheritance of the 
properties of basic building block objects. These are then used to simulate any 
number of platforms of that class, each operating independently within the simulation. 
The computer memory available to store each of these instances of the object becomes 
the limiting factor to the complexity of the scenario to be examined. The organization 
of objects in ASSET consist of several major groups: 



• Graphical environment objects which implement the standard Macintosh 
graphical user interface to provide input/output by means of a mouse based 
"point and click" metaphor featuring pull-down menus,dialog boxes with labeled 
data regions and radio buttons, and windowed presentation of multiple 
information sources simultaneously. 

• Region management objects which construct and manage distinct regions of 
varying environmental properties, command responsibility or platform patrol 
assignment. 

• Event management objects which queue all simulation events in time order 
sequence and parcel them out to the appropriate resolution objects. 

• Command objects which perform resource allocation, assigning available assets 
to contact cues based on either time to station (MPA) or area of uncertainty size 
(submarines). 

• Automatic Correlator Tracker (ACT) objects which manage the grouping and 
processing of contact reports into target tracks creating a tactical picture 
consisting of combinations of true and false contacts. 
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• Tactical platform objects which simulate the behavior of their real world 
counterparts. These are dynamic associations of component objects which may 
change in response to simulation events. A submarine inheriting the properties 
of a Patroller object may switch to an Interceptor after making a detection. 

The program flow from object to object is not sequential, but event driven. 

Once started, the simulation clock advances time relative to the simulation. Sensor 
objects begin glimpsing and motion platforms begin moving. Possible detection 
events are sent to the event manager for proper sequencing and resolution. Detections 
are communicated up the designated chain of command to data-fusion centers where 
an instance of the ACT processes them and integrates them into the tactical picture. 
This may cause a command object to direct (or redirect) assets in response. This 
process continues until the allotted time for the scenario expires. The MOE statistics 
for that run are added to the MOE file and the next iteration of the scenario 
commences. When the desired number of iterations are complete the compiled 
summary of MOE statistics is analyzed to interpret the results of the simulation. 

2. ASW System Architecture Evaluation. 

In order to evaluate a desired architecture it must be conceptualized in great 
detail. The classic computer maxim "garbage in, garbage out" is especially true of 
ASSET where a single bad parameter value can render the results meaningless. 

While the user-friendly interface facilitates the mechanical process of inputing data, 
the abstract nature of the required parameters make it necessary to assume values of 
dubious validity at times. General areas which require quantified data include: 
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• Communication connectivities which represent how detection reports are passed 
from object to object and what delays are involved at each node. 

• Command organization including geographic regions of responsibility. 

• Environmental data for acoustic propagation loss as a function of range and 
frequency as well as ambient acoustic noise in each distinct environmental 
region. 

• Motion plans representing the operating areas and interconnecting tracks which 
will govern tactical platform movement. 

• Umpire parameters such as the kill probabilities for a given submarine class 
against each possible target class both for the case where it detects the enemy 
first and the case where it is detected by the enemy first. 



The individual platforms require complex definition as well. A submarine 
object, for example, requires parameters for: 



• breakoff speed between "fast" and "slow" acoustic behavior. 

• self-noise at slow and fast speed. 

• directivity index and recognition differential (for Sonar Equation computations). 

• signature frequency emitted and intensity at slow and fast speed. 

• movement speeds when patrolling and intercepting and detailed motion plan to 
be followed. 

• weapons loadout and level at which the sub will abort its mission to rearm. 

• whether the sub will transmit the detections it makes and risk detection itself or 
not report any detections. 

• the interval at which it copies the submarine broadcast for orders to investigate 
cues. [Ref. 2:p. 2-37] 
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The submarine’s sensor requires a detailed set of parameters of its own which 
will be used to construct the contact report that is communicated to the ACT 
representing the data fusion center associated with the submarine’s command object: 

• position error which represents the 2 standard deviation radius of a circular 
normal distribution of contact errors about the actual target position. 

• course and speed errors which are uniform about the true value. 

• Target Motion Analysis (TMA) delay representing the time required to acquire 
course and speed information. 

• false alarm rate. 

• determination whether, on the average the sensor is reliable enough for the 
fusion center to automatically start a new track based on a single contact report. 

The complexity of the data involved in constructing an architecture for 
evaluation varies considerably, as can be seen from these examples. Once all this 
information is input, it can be easily double checked by accessing the appropriate 
windows again. When the user is sure all data is input correctly, the simulation can 
be run with the platform graphics turned on to ensure it progresses properly. These 
graphics can then be turned off to run multiple iterations faster. 

When the desired number of iterations is completed a dialog box opens asking 
if the MOE’s are to be saved. Once saved to disk, they can be opened and examined. 
They include: 
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• Attrition of submarines and MPA assets for each side as a fraction of original 
force. 

• Number of times submarines approached to within a critical range (assigned by 
the user) of enemy surface formations (which serve only as targets in ASSET 
1.0, they have no combat or detection capability). 

• Tracker statistics involving the fusion delay involved in constructing target 
tracks. 

These statistics along with weapon expenditures allow the completed simulation 
to be compared to other runs to observe the effect of a particular parameter changing, 
or how close a particular MOE comes to a goal value. The methodology of ASSET 
must be taken into account as the computer commanders follow simple resource 
allocation rules which do not necessarily reflect the tactical priorities the user desires. 
A higher priority for prosecution may well be given to a submarine returning to base 
than one ten miles from a surface group, due to the resource allocation process’ 
emphasis on detection rather than on protection. While creativity is needed in the 
design, implementation and interpretation of an ASSET scenario, significant insights 
into the conduct of ASW campaigns can be gleaned from it. 

3. Correlation, Tracking and Data Fusion. 

ASSET is designed to simulate the data fusion centers where contact reports 
from a wide variety of sensors and platforms are centrally processed to create a 
tactical picture for the region of interest. These data fusion centers are simulated by 
an instance of the OBU-ACT module with appropriate communication connectivities. 
The result of correlating and processing the streams of contact information arriving at 
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a fusion center forms the basis for the allocation and cuing of assets. Thus it is 
critical that the ACT is not spinning its wheels performing unnecessary computations. 

The implementation of OBU-ACT used in ASSET is organized into four main 
functional areas: 

• generating a new track from one or more contact reports (the START and 
CLUSTER modules). 

• associating a contact report to an existing track (the INPUT, START, and 
ASSOC modules). 

• simulating the intervention of a human analyst to resolve ambiguous contact-to- 
track associations (the ANALYST module). 

• updating and managing a database of the status of all contact reports and tracks 
(^e Locational Data Base Manager and MTST modules). 

The data flow through these modules is shown in Figure 1. A detection is made 
by a sensor and a report is initiated. After the applicable delays have elapsed, the 
contact report travels along the communications network to the fusion center, 
experiencing additional delay at each node. 

Upon arrival at the fusion center the INPUT module enters it into the Locational 
Data Base Manager (LDBM) module and passes it to the ASSOC module. There the 
contact is compared to the existing tracks on the basis of geofeasibility. Those tracks 
with which the contact could be associated are evaluated by a statistical comparison of 
a measure of correlation (MOC) with a preset threshold. If the contact fails to trigger 
an association with my track it is passed to the START module. If the contact 
triggers a single association, or if one track MOC exceeds all others by a preset 
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Figure 2: ASSET contact data flow. 



margin, it is unambiguously associated with that track [Ref. 4:p. 3]. Ambiguous 
association is resolved by the ANALYST module, which has a given probability of 
making the correct association (.7) and an associated exponentially distributed delay 
time spent making the decision (mean of .2 hours) [Ref. 3:p. 11; Ref.5: ACT- 
ANALYST]. 
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The contacts which are unresolved are passed to START which checks the value 
of the flag single-report-to-track. If the value is true, START initiates a new track 
based on the contact, if it is false, the contact is passed to the multiple contact track 
initiation module CLUSTER. This represents a simplification of the actual OBU- 
ACT START module which uses comparison to a set of criteria and human analyst 
interaction to determine if a new track is to initiated [Ref. 6:p. 3-4]. Within the 
context of the simulation, this simplification makes sense as two classes of detecting 
sensors are represented; those that report continuous contact observations such as 
SOSUS or other fixed area sensors, which are processed at the fusion center; and 
those which perform platform-level analysis and periodically report the tracks they 
hold, such as submarines. Platforms in the first category, unless possessing 
unusually low false alarm rates, would not initiate a track based on a single report, 
while those in the second, would. 

Unassociated contacts which remain after passing through the ASSOC and 
START modules are processed by the CLUSTER module. Here subsets of all 
unassociated contacts are evaluated for geofeasibility and constant course and speed 
likelihood. If the value of the constant course and speed likelihood exceeds a 
threshold value then the subset of unassociated contacts is used to initiate a new track 
[Ref. 7:p. 1-3]. Contacts which are not associated after passing through ASSOC and 
CLUSTER carry counters which increment each time they pass through the two 
modules. When these counts exceed maximum limits (hardwired at 7 in CLUSTER 
and at 5 in ASSOC) they are dropped from the LDBM [Ref. 5: OBU-ACT]. 
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When a new track is initiated or a contact is associated with a track, the MTST 
module is called to filter the track data and produce an optimal estimate of the target’s 
location and velocity along with the error covariance matrix describing the quality of 
the estimate. The full capability tracker in MTST contains the matrix versions of the 
Kalman filter equations. These equations determine the mean target position at a 
future point in time and the covariance matrix representing the error in that 
prediction. The elements of this covariance matrix can be recast as ellipses 
representing two a or 86 percent containment regions for the target’s possible position 
and velocity. A complete description of MTST is the subject of Chapter n. 

The foregoing process of correlating contacts to tracks and filtering those tracks 
to provide an estimate of target position occurs at each instance of a data fusion 
center object. When formulating an architecture, the fusion center’s role must be 
carefully designed. Since no contact reports flow out of a fusion center, it is not 
possible to directly fuse the pictures at two or more fusion sites into a higher echelon 
center. The individual sensors originating the detection reports must send duplicate 
reports to all fusion centers which will be using the report in determining a tactical 
picture. The fusion center also keeps separate instances of the tracker to process 
surface contacts and subsurface contacts. Together with the START processing of 
single contact to track decisions, this assumes that a great deal of contact processing 
is going on below the level of the fusion center. 

Assuming that reports arriving at the fusion center are pre-processed justifies 
several assumptions that the DETECTION-REPORTER object makes about the 



14 



reports it generates. Most important to streamlining filter operation is the assumption 
that all submarine probability areas (SPA’s) are circular. While this represents a 
considerable inaccuracy for single contact line-of-bearing detections, if one assumes 
the contact report represents the accumulation of a significant number detections, the 
circular error assumption is more reasonable. Several types of detections are resolved 
in ASSET without explicitly modelling the geometry of the searcher and target at the 
time of detection (MPA and Fixed Area Sensors specifically). The calculation of the 
size and orientation of explicit AOU ellipses where possible would slow execution and 
add considerable complexity to the program, requiring even more specific user input 
sensor parameters. 

The assumption that this circular AOU size is independent of range is more 
difficult to defend, in light of an expected linear relationship between the standard 
deviation of the position error and range. That assumption on ASSET’S part can be 
remedied easily, for the cases where range is explicitly determined, by expanding the 
radius of a sensor’s AOU linearly with range based on a user input bearing error. 

This would make the process of tabulating preprocessed variance data impossible, 
however, as the sensor AOUs would no longer be constant. A good compromise 
involves defining three AOU sizes for contacts at short, medium and convergence 
zone ranges. This would allow for some representation of the range effect on AOU 
size, with acceptable complication of the variance pre-calculation procedure. 

The other tracking assumption ASSET makes is that the covariance of the 
velocity can be represented by the steady-state stationary cross-covariance of the lOU 
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process, independent of the sensor characteristics. This assumption is used to 
initialize the covariance matrix of a new track. This method is correct for a position- 
only contact which gives no information about velocity. Given position and velocity 
information however, this is only accurate if the sensor is so inaccurate that the lOU 
process velocity variance is small compared to the sensor velocity variance [Ref. 8:p. 
5 - 20 ]. 

This is not true in most cases however, with the result being that the filter re- 
initializes the velocity update each time a contact is processed, never including any 
representation of the velocity accuracy of the sensor involved. Thus, the velocity 
variance is based solely on the lOU model and not on the variance of the velocity 
reported by the sensors. This assumption makes the tracker more responsive to 
course changes, but it also artificially inflates the SPA size of accurate contact 
streams because it ignores the sensors velocity error, which may be significantly less 
than the lOU value. 

Given these assumptions and the noise characteristics of the lOU process, the 
filter implementation can be streamlined to better match the data provided. This 
process is detailed in the following chapters which will describe the lOU process and 
the formulation of the general and ASSET-specific filter equations. 
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n. THE MANEUVERING TARGET STATISTICAL TRACKER 



Maneuvering Target Statistical Tracker (MTST) is the name given to a class of 
Kalman filter-based estimation and prediction algorithms utilizing the Integrated 
Omstein-Uhlenbeck process to model the statistics of target motion. To successfully 
track a maneuvering target, the details of the target path should be statistically 
predictable. How accurately an automated tracker like MTST performs its task is 
directly related to how well the tracker’s target motion model describes the targets 
actual random motion. 

The random tour model [Ref. 9] can be used to describe the statistical properties 
of a target moving at constant speed which makes random course changes at times 
separated by intervals chosen from an exponential distribution. This provides a 
reasonable estimate of the type of motion expected of a patrolling submarine target. 
Unfortunately, the target distribution generated by a random tour is not normal and 
cannot be used directly with a Kalman filter [Ref. 8:p. 2-10]. A process is required 
which produces normal distributions which best approximate the mean and variance of 
the random tour. This is the lOU process. The mathematical development which 
follows is summarized from the more complete treatment in Reference 8, the results 
of which are in agreement with References 5, 10 and 13. 
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A. THE INTEGRATED ORNSTEIN-UHLENBECK PROCESS 



The Integrated Omstein-Uhlenbeck Process (lOU) is based on a first order 
stochastic linear differential equation which approximates the higher order non-linear 
deferential equation which defines the motion of a randomly maneuvering target. It is 
a member of a class of equations known as Langevin equations and has the form; 

—u(,t) = ~^u(t)dt*adw(t). (2.1) 

dt 

The first term represents a deceleration caused by a resistive force proportional to the 
velocity u(t). The random forces acting on the particle are represented by the second 
term, where w(t) is a Gaussian white noise process. 

The stochastic nature of this equation makes it possible to find only the statistics 
of the distribution of the solutions, rather than the specific solution itself. A Gaussian 
w(t) produces a Gaussian velocity process u(t) which, like any normal process, is 
defined completely by its first and second order moments. The result is the velocity 
distribution of a particle which is undergoing random motion similar to Brownian 
motion, experiencing random instantaneous accelerations, but whose velocity is 
damped by a spring-like effect which constantly accelerates the particle in the 
direction opposite its velocity at a rate proportional to that velocity. The position 
variance of such a particle is unbounded over time, but the velocity variance is 
limited by the damping coefficient /S. The result is a velocity distribution that is 
normal with a limiting variance given by: 
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( 2 . 2 ) 



UmV'{«(/)} - lira £{[«(»)-£ {«»}]1 = 4- 

(-♦00 f -»00 zp 

and a mean for any given t given by: 

£{m(0} = e-^E{u{Qi)}. 

The mean and variance expressions for the position of an object whose motion 
is governed by an lOU process completely specify its positional distribution over 
time. In order to show that the lOU process approximates the same motion as a 
random tour, the radial position distributions for the two stochastic processes will be 
shown to have the same variance. 

B. EQUIVALENCE OF THE RANDOM TOUR AND lOU PROCESSES 
The variance of the radial distance from {x(0),y(0)) is: 

For the Random Tour, the following holds: 

£{«"«) = (2.5) 

a* 

where V is the speed of the randomly touring particle and a is the mean number of 
course changes per unit time. The corresponding result for the lOU process is: 
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£|«V)1 = 

where a is the scale factor for the random acceleration and fi is the damping constant 
on the velocity, as in Equation (2.1). 

These two equations can be made identical given the following relationships 

hold: 

^ = a ^ (2.7J 

Thus, a Random Tour with parameters a and V can be approximated by an lOU 
process with parameters /3 = a and a — It is worth noting that since a normal 

distribution is completely specified by its mean and variance, the lOU process 
represents the best normal approximation to the Random Tour [Ref. 10]. 

In ASSET the lOU parameters, 0 and a are hardwired to represent a target 
conducting a random tour at ten knots with an average time between course changes 
of four hours. These parameters should match the actual motion of the platforms 
modelled in a given scenario. Since the user can choose whatever target motion 
parameters he wants for each individual platform, the parameters embedded in 
ASSET may disagree considerably with the actual target motion. This disagreement 
causes the lOU prediction to consistently lead or lag the target’s mean position, 
depending on whether the model’s speed and course change rate correspond to a 
velocity faster or slower than the target’s speed, respectively. This causes the 
position of the center of the resulting SPA to be offset from the true target position. 
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Even when the tracker parameters exactly match the target motion, a considerable lag 
occurs if the time between measurements is not significantly greater than the time 
between maneuvers. 

C. OTHER MOTION MODEL OPTIONS. 

The lOU model used by MTST damps out the mean speed of the target 
exponentially over time to account for target maneuvers. No matter how small the 
time interval, the process reduces the mean speed by the proper amount. If the 
interval between detections is consistently shorter than the maneuver interval, the 
damped velocity is used to predict the position of the constant velocity target and the 
tracker develops a lag. One remedy for these inconsistencies would involve 
increasing the complexity of the tracker by using adaptive methods to more closely 
match the model parameters to a given target’s track. An adaptive filter recognizes 
changes in the targets motion and compensates for it by changing the parameters of 
the motion model. 

Desiring to reduce the filter’s complexity however, instead of explicitly 
modelling an adaptive tracker, the use of such a tracker can be approximated using 
the lOU process as a basis for the size of the SPA, but assuming the model has some 
adaptive properties to position it more accurately. This can be accomplished using 
the existing lOU model to estimate the SPA variance and by assuming the tracker’s 
position prediction’s are distributed about the target’s true position in a circular 
normal fashion. This is an optimistic assumption, but captures the fact that real world 
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trackers, using more complete contact data than ASSET and an adaptive motion 
model, can achieve more accurate position and velocity estimates. Examinations of 
such trackers can be found in References 14, 15 and 16. Reference 14 in particular 
demonstrates a passive tracker that delivers outstanding accuracy while the contact is 
on a constant course and speed, but has difficulty maintaining track during difficult 
target maneuvers. 

The benefits of adaptive tracking are not applied to the SPA size in this case, as 
the ASSET tracker is not an adaptive one, but are assumed in simplifying how its 
position is determined. This technique introduces a moderate degree of error in SPA 
size by not precisely simulating the mechanics of distributing the position of the 
center of the SPA. It also does not take into account the filter’s velocity components. 
Reference 15 provides methods for countering these effects through noise adaptation 
and correlated maneuver gating while Reference 16 uses bias-sensitive maneuver 
detection and Kalman gain adaptation. Another option used to improve the lOU 
model is the use of a dual velocity system which combines a short term lOU process 
combined with a long term one. This type of model can reproduce a variety of 
motions depending on the weighting factors given to the short and long term 
components. The resultant Kalman filter has six states and a 6 x 6 covariance matrix. 
This is the implementation of the lOU process currently used in the TOMAHAWK 
fire control system for surface ship motion modelling [Ref. 8]. These are but a few 
of the schemes more complex trackers can use to improve motion modelling. The 
single velocity lOU process, while not adaptive, comes close to satisfying the 
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necessary conditions and since it is the basis of the ASSET tracker will be used for 
comparison. 

D. THE KALMAN FILTER 

In order to estimate target position, a method of combining the contact 
position and its covariance with a prediction of that position and covariance based on 
the lOU process is necessary. The Kalman Filter provides such an estimate, provided 
the process model is linear and time invariant, which the lOU process is. The 
Kalman filter is derived from the least-squares estimation theory of Gauss and 
Legendre [Ref. ll:p. 7]. This technique sought the most probable value of an 
unknown quantity based on a series of measurements containing unknown errors. 
More formally, finding the most probable estimate of x based on the matrix equation: 

where vector Zt represents a measurement taken at a discrete time k, based on an 
observation matrix H revealing information about one or more state parameters of the 
true state vector x but corrupted by a noise vector v. This technique was generalized 
by Kalman to apply to linear filter theory, producing a filter which produced a best 
linear estimate of x [Ref. 12]. 

The resulting filter is based on two mechanisms operating sequentially, one 
predicting the state and covariance for a future time and the other combining this 
prediction with a noisy measurement of the target’s state taken at that time. This 



23 



should, given correct motion modelling and noise parameters, produce a state and 
covariance which more accurately reflects the true target state than either the 
prediction or the measurement alone. 

These mechanisms are represented by two systems of matrix equations. The 
variables involved are defined in Table 1. The prediction equations for the state x 
and covariance £ are: 

state prediction: = <l>x^+n^, (2.9) 

covariance prediction: + Q^, (2.10) 

where <f> represents the transition matrix from the dynamic system model, Q 
rq)resents the variance inherent in the dynamic model and represents the mean of 
the noise term w from the dynamic model, which is assumed to be zero in ASSET. 
The equations for combining the predicted and measured states and variances require 
the computation of a Kalman gain matrix which provides the weighting factors for the 
combination. These equations which update the filter’s estimates are: 

Kalman Gain: 

( 2 . 12 ) 

state update: x*,, = + K*,, [z,,, 

covariance update: (2.13) 

where R represents the variance of the noise inherent in the measurements, Z is the 
measurement itself, py is the mean of the noise inherent in the measurement (also 
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Table I: Kalman Filter Definition of Terms 



Kalman Term: 


General Form 


ASSET Form 


State vector at time k\ 


X* n X 1 


mean 4x1 


State transition matrix, 
time k to time A:+i: 


n X n 


phi 4x4 


State process noise mean: 


Mw n X 1 


assumed zero 


State error covariance, 
at time k: 


n X n 


var 4x4 


State process error 
covariance at time k: 


Qk n X n 


f 4 X 4 


Observation matrix: 


H m X n 


14x4 


Observation at time k: 


m X 1 


state 4x1 


Observation error mean: 


/*v m X 1 


assumed zero 


Observation error covariance 
at time: 


m X n 


covariance 4x4 



assumed to be zero in ASSET) and I is the identity matrix of proper size. The state 
and covariance updates, assuming H (as ASSET does) is the appropriate identity 
matrix can be written as: 



'*.1 ^*.1 '*'^**1 


P.14) 




(2.15) 



where the inverse of and are the weighting matrices. Thus the inverse of 
the noise error variances are the weighting factors by which the prediction and 
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measurement are linearly combined. This is the form of the Kalman equations which 
ASSET actually uses. The advantage of this form is the ready understanding of the 
underlying mechanism of weighted linear combination. The disadvantage is the 
computational necessity of performing three matrix inversions to calculate the 
variance propagation. Regardless of the form of the equations, the <l>, Q and R 
matrices must be developed to complete the computational form of the filter. 

1. Development of the ^ Matrix. 

The matrix <j> represents the transition matrix which governs the dynamics of the 
state between two discrete times. The following development suihmarizes the 
derivation of Reference 11, Section 5.2. The differential equations defining the lOU 
process and velocity can be combined to form a single matrix differential equation 
which takes the following form in the single coordinate direction x: 







0 


1 




’x(t) 




0 






_0 






_u{t)_ 




a 



w(r) 



(2.16) 



more compactly. X(r) = F(r)X(r) + Gw(/) (2.17) 

where Xfirjrepresents the system state vector and the dot over a symbol indicates 
differentiation with respect to time. Given that X(t) has the value X(tJ at time t^ then 
X(t) at any future time t>to 'is given by: 
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(2.18) 



X(t) = Ht,OXiO* \ 4>if,r)Gw(j)dr 



where represents the transition matrix between the states at time and time t. 
Provided that F(t) is constant in time and assuming that is zero, the matrix <f> can be 
shown [REF. 8:p. 5-3, 5-5] to have the form: 



1 



4>(0 = 






0 



e-^‘ 



(2.19) 



This represents the transformation projecting a state into the future based on the lOU 
model. The velocity mean decays to a new value which is simply the old value 
multiplied by e'^‘. The position is moved a distance equal to the time 1//3 multiplied 
by the change in velocity: 



P 

where: 



( 2 - 21 ) 

The form this takes when expanded to cover both coordinate directions is: 
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1 





1 



0 




( 2 . 22 ) 



0 

0 



0 

0 



0 



0 



where t is time elapsed from the last measurement to the observation time of the 
current measurement. It is important to notice that the transition matrix depends only 
on the lOU parameter /3 and not on o. Thus the lOU velocity damping coefficient 
and the elapsed time determine the dynamics of the mean state and variance 
transition. Appendix A contains graphs of the values ^(1 2), which will be called 4>2, 
and 0(2), which will be called 03, take over various time intervals. The noise 
magnitude coefficient o is involved in the noise terms R and Q. 

2. Development of the Q Matrix. 

The Q matrix represents the noise introduced by the white Gaussian process 
w(t). As discussed above, as the elements of this matrix get large the filter 
increasingly relies on the measurement data and ignores its predictions. As the 
elements get small the filter increasingly ignores the new contact data and the errors 
in its predictions compound until it diverges, completely losing track of where its 
target is. The definition of this matrix, the process noise error covariance, is: 



Q = e[^Z-£(Z)}1;!-E(Z)]’'1 



(2.23) 



which for the random process operating in (2.26) becomes: 
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t 




t 


T 




f <f>(t,T) Gw{t)dr 




f <l>(t,s) Gw(s) ds 


> 




k 




_k 





(2.24) 



Since the mean of the white Gaussian processes w(t) and w(s) are both zero and the 
variables of integration t and s are equal, this reduces to the form: 



Q- { GG^ <l>^(t,T)dT. (2.25) 

Substituting the appropriate matrices G and <j> from (2.25) and (2.29) respectively and 
performing the indicated matrix multiplication results in: 



Q = 



/ 








^(l.g-«/-T))g-«/-T) 






dr 
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for the single coordinate direction x. Assuming that /„=0 and recognizing that q(l,2) 
is equal to q(2,l) results in three expressions to integrate to get the final value of the 
Q matrix in one direction: 



Q = 



qll 

q21 



ql2 

q22_ 



where ql2 - q21 



qll 



— 2t-l(3-4e-^ + e-^^') 
2 / 3 ^ 



(2.27) 



(2.28) 
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(2.29) 



ql2 ‘q2I = ^(l - 2e 

«22 = ^(l-e-») (2.30) 

When projected into two coordinate directions the full representation of the Q matrix 
is: 




0 

qll 

0 



ql2 0 ■ 
0 ql2 

q22 0 



0 ql2 0 q22 



a.3i) 



Each element in this matrix is directly proportional to and decreases with 
increasing Time has an inverse exponential relationship causing the noise to 
increase the longer the interval between measurements. At t equal to zero the noise 
terms are equal to zero and as t goes to infinity : 



ql2-—, q22-—. (2.32) 

2p2 P 

As mentioned previously, the variance in position is unbounded, but the cross- 
covariance and velocity covariance are bounded. These values become important in 
developing the measurement noise covariance matrix R. Appendix A contains graphs 
of these values over various time intervals. 
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3. Development of the R Matrix and Filter Initialization. 

The matrix R represents the covariance of the measurement noise and 
accompanies each state measurement reflecting its precision. This value may vary 
with time in the ASSET tracker as contact reports flow in from a variety of sensors. 
The inverse of this matrix is used as the weighting function for the contact report, the 
larger the elements of R the less effect the contact has on the filtered update. The R 
matrix also plays a role in initializing the filter. 

When a target is first detected, there is no t-1 state for the filter to work from in 
making its prediction. In general, a time r„ R matrix is preset to initiate the filter. 

This matrix would consist of the 2 x 2 positional error matrix which describes an 
estimate of the elliptical AOU about the contact position: 



1 (A^ cos^O) +(B^ sin^O) (B^ - A sinO cos6 

4 {B"^ -A^) sin0 cosd {A ^ sin^0) + (B^ cos^^) 



(2.33) 



For a position-only contact, the two lower right diagonal entries would be set equal to 
the stationary variance of the lOU process (2.2). This represents the maximum 
variance the velocity could possibly have based on the motion model and since no 
velocity information is coming in, the filter is relying solely on the motion model, 
making this a value a reasonable choice: 
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EQ. 2.33 



0 

0 



0 

0 



(2.34) 



0 0 ^ 0 - 

0 0 0 

If the contact is a position-and- velocity contact, the procedure is more complex. In 
like fashion to the initialization of the upper left 2x2 portion with the variance in the 
sensor (its elliptical AOU), so must the bottom right 2 x 2 be initialized by the 
velocity variance of the sensor. Since the lOU process is predicting the velocity 
distribution also, this variance of the measurement must be combined with the 
variance of the lOU process. The proper combination can be shown [Ref. 8:App. F] 
to be the same as the alternate form of the Kalman covariance update equation (2.14). 

This method is not used by the tracker in ASSET. The ASSET tracker simply 
uses the initial contact’s lOU velocity variance as the basis for a new track, without 
updating it. This original contact report is used as the t-1 state and covariance when 
the next contact associated with the track comes in. The contact reports themselves 
also differ from the construction described above. The positional variance of the 
sensors are all circles in ASSET so the 2 x 2 matrix which occupies the upper left of 
the R matrix is actually the 2 x 2 in the upper left of: 
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/•2 

— 000 
4 

0 L 0 0 

4 

0 0^0 

2a 

0 0 0 ^ 

2a 



(2.35) 



The velocity variance in the lower right hand 2 x 2 is correctly set for a position-only 
contact, but this is also used for a position-and-velocity contact. Thus the R matrix 
which ASSET uses never incorporates the error in velocity inherent in the sensor. 
Using these values essentially reinitializes the filter each time a measurement is made. 
This is desirable for position-only contacts as it prevents divergence of the velocity 
components of the state, however for position and velocity contacts, this can cause 
poor velocity accuracy as the tracker improperly weights the velocity components of 
the contact report. The velocity information of different sensors are given equal 
weight regardless of the relative size of the errors in the reports they make. The 
actual velocity variance of the sensor should be computed and used to set the value of 
the filtered velocity using the form of (2.15). 
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m. IMPLEMENTATION OF THE TRACKER IN ASSET 



Having developed the components of the Kalman filter, the construction of the 
tracker implemented in ASSET can be discussed. The following is based on 
Reference 5, the ASSET source code for version 1.0. The ACT-MTST module 
consists of objects which perform the filter calculations described above, as well as 
objects which maintain the spatial relationships between the positions involved on a 
spherical earth. 

There are several constants which are defined for use in all the module’s 
objects. The first is dtor which is used for degree to radian conversion. Where 
trigonometric functions are indicated below, the actual code uses dtor to convert 
angles from degrees to radians, but in the interest of brevity, this will be excluded. 
Second is a, which is equivalent to /3 in the lOU process, and is set to .25. Third is 
<x which is the noise scale factor from the lOU process as described above and set to 
VSO. This is equivalent to the square root of a velocity multiplied by the Random 
Tour velocity. Thus the tracker predicts the target position by assuming a random 
tour is being conducted at a speed of ten knots with a mean interval between course 
changes of four hours. This cannot be changed by the user. 

Inputs to ACT-MTST are read from the data in ACT-LDBM, the contact 
database manager. If a new track is to be initiated, the next MTST object, start-new- 
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track sets the track head and track tail state and covariance matrices equal to the 
contact mean and covariance using compute-mean-covariance. The use of two sets 
of data, a head and a tail, to allow incorporation of out-of-sequence contact reports 
will be discussed below. The object compute-mean-covariance is also called by 
ACT-ASSOC as a basis for the spatial measure of correlation used to determine 
contact to track correlation. 

Compute-mean-covariance creates two arrays, a four by four called var, which 
is the R matrix, and a four by one called mean, which is z^: 



mean 



0 

0 

spd cos(90-cse) ’ 
spd sin(90-C5e) 



(3.1) 



and var, which has the same form as (2.34). The vector mean is centered at the 
latitude and longitude indicated in the contact data list and x and y velocity computed 
from the contact course and speed. This variance matrix however, as discussed 
above, is calculated as if the contact report consists of an ellipse with minor axis B, 
major axis A and orientation angle 6. All the spatial data fields that are generated by 
the Detection Reporter and Generic Sensor contain a single positional uncertainty and 
an orientation angle of zero. Modifying (3.1) to take advantage of this fact results in 
(2.35) where r is equal to the standard deviation of the sensor’s target position 
estimates. This alternative form returns matrix element values identical to those 
computed by (3. 1), given the actual form of the contact reports generated. 
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If a contact is associated with an existing track rather than used to start a new 
one, the update-track object is called by ACT-LDBM to perform the Kalman filter 
operations. The remaining objects in ACT-MTST are all components of the update- 
track object with the exception of the object cov-to-ellipse, which is called by various 
display and graphics objects to determine the orientation, semi-major and major axes 
of the ellipse representing a covariance matrix. Since, as shown above, all the 
ellipses are actually circles, this object is another anachronism of the mismatch 
between the capabilities of the tracker and the contact reports actually input to it. It 
can be simplified in a manner similar to compute-mean covariance above to return 
the radius of a circle instead of a major axis, minor axis and orientation of a ellipse. 

Focusing once again on the update-track object, it first defines the spatial data 
parameters of the contact and the track the contact is used to update. ASSET uses 
four data structures to represent the data associated with contacts and tracks: 



• obu-contact: consisting of contact id, receipt time, track association, sensor, 
categorization (track association flags "pinn^p" and "lockedp", and number of 
passes through ACT-CLUSTER and ACT- ASSOC), spatial data (the obu-data- 
field described below), altitude (surface or subsurface) and HFDFp (an optional 
parameter associated with HFDF contacts). 

• obu-data-field: type (position-only or position-and-velocity), observation time, 
latitude, longitude, AOU orientation, AOU major axis, AOU minor axis, 
course, course uncertainty, speed and speed uncertainty. 

• obu-track: track id, number of contacts associated to the track (maximum of 
five), state and covariance of the track head (most recent contact) and track tail 
(oldest online contact), head and tail spatial data (each an obu-data-field), head 
and tail contact id, and altitude (surface or subsurface). 
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• obu-state-field: reference latitude, reference longitude, state array (offset from 
reference position in miles N/S, offset from reference position in miles E/W, 
N/S velocity, E/W velocity) and covariance array (four by four array per (3.1)). 

The three AOU variables can be eliminated from obu-data-field and replaced by 
position-uncertainty, the radius of the actual AOU circle reported. This will not 
affect the tracker’s calculation and reduces the memory taken up by contact reports. 

An obu-track consists of up to five contact reports, the most recent of which is 
the track head and the oldest is the track tail. The variables required for performing 
the filter calculations are read from the data structures described above, update-track 
then checks the time since the last update. If the new contact observation time is later 
than the track head update time, the contact is used to update the track head using a 
set sequence of object calls. This sequence contains the Kalman filter procedures as 
well as two routines which maintain the latitude and longitude relationships on a 
spherical earth and proceeds as follows: 



• ioumotion: Performs the lOU prediction step and updates the N/S and E/W 
offset distances in the track head state to the time of observation of the contact. 
These offsets effectively perform the prediction in a plane tangent to the earth’s 
surface at the reference latitude and longitude point of the track head. 

• change-tangent: Finds the relationship of the iou predicted position and the 
contact point in a plane tangent to the earth at the contact’s reference point vice 
the track head’s reference point used to compute the prediction in ioumotion. 
These matrices are stored as state-cov-old. Since the covariance is circular, 
there is no need for the sequence which rotates it. 

• make-state: Takes the contact data and produces 4 which is called simply 
mean and its covariance R which is called var. These two matrices are stored 
in state-cov-ctc. 
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• filter: Takes state-cov-old and state-cov-ctc and uses Equations 2.14 and 2. IS 
to compute x^+i and The result is the Kalman filter update performed in a 
plane tangent to the earth at the contact’s reference point. 

• center-tangent: The latitude and longitude of the position produced in filter is 
projected back onto the earth from the tangent plane. 

• update-track-head: The new updated state and covariance are made the new 
track head. The contact used to perform the update provides the track-head 
contact data. 

• update-track-tail: Sorts the contacts making up the track in time order, oldest 
to most recent (1 to n). If there is only a single contact, it is designated the 
track tail. If there are five or fewer contacts and the oldest contact is already 
the tail it remains so; otherwise the oldest contact is made the new tail. If there 
are more than five contacts, it takes number contacts, subtracts five and the 
contact whose number equals the result is used to update the track tail using the 
procedure detailed above. The contact used to update the tail provides the track 
tail contact data. All contacts older than the tail are then pruned from the 
track. 



Considerable computation time can be saved by simulating a portion of the above 
process, rather than actually carrying out all the steps. A revised update procedure 
based on using the updated variance as an approximation to the distribution of 
updated states (Xk.«.i) is proposed. Rather than produce the contact position latitude 
and longitude randomly using the sensor variance as a distribution and processing it 
using the sequence above, the latitude and longitude are chosen by computing the 
updated variance, which can be done prior to scenario execution. When it is needed, 
the distribution is centered on the target’s true position, and used to generate the 
updated states directly by random draw. This new procedure analyzed in detail in 
Chapter IV. 
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Time delays incurred at communications nodes enroute to the fusion center may 
cause the observation time to fall between the track head update time and the track 
tail update time. In that case a modified procedure is followed . 



• If the time of observation for the contact is later than the tail update time or if 
the track does not yet consist of five contacts, then the contact is appended to 
the track and the contacts are sorted by time of observation. If the contact is 
older than the tail and the track already consists of five contacts, it is ignored 
and the procedure stops. 

• If the observation time is later than or equal to the tail update time, the tail is 
used as the oldest contact, if the new contact is older, then compute-mean-var 
is used to make the new contact the oldest. 

• The oldest contact is then updated using the procedure described above using the 
next contact in the track, in time sequence, as the "contact". The result of this 
becomes the "old contact" and the procedure is repeated, updating each contact 
in the track in sequence until the head is updated. The resulting "forward 
filtered" track closely approximates the track head which would have resulted if 
the time-late contact had been received in sequence. 

The calculations performed in ioumotion and filter, all assume that elliptical 

AOU’s are being produced by the Detection Reporter and General Sensor objects. 

The object make*state can be simplified in the manner as compute-mean-cov as it 

performs the same operations. The reduced forms of the equations in those objects 

are computed below. 



A. REDUCED COMPLEXITY lOU PREDICTION 



The matrix equations for calculating the lOU predicted state and covariance are 
given by Equations 2.9 and 2.10. Using the symbolic processing capability of the 
MathCad 3.0 (copyright 1991 MathSoft, Cambridge MA) computer software package. 
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the matrix operations were evaluated. The large number of zero terms and high 
symmetry of the <t> and Q matrices resulted in simple expressions for the elements of 
the state and variance predictions. Portions of the calc-mot-mat object which 
compute f(0,0), f(0,2), f(2,2) for positive time as well as 0(0,2) and 0(2,2) can be 
used to produce the terms of the following equations used to compute new-var: 



where: 



where: 
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and: 
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and with: 
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(3.6) 



The /terms given here are the ASSET versions of (2.36), (2.37) and (2.38). 
B. REDUCED COMPLEXITY KALMAN UPDATE 



In a manner analogous to that used in the preceding treatment of the lOU 
update, the matrix equations for the Kalman update were evaluated. The result is: 



where: 



Kalman Gain - 



K“,., 0 Kbl,„ 0 

0 J&... 0 KbJ,., 

Kb2,., 0 0 

0 Kb2,., 0 JCC... 



(3.7) 
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XW... =i [v%..,lr^j , 

X»2... 

Xc... [''c,.,(V^a..,*lr^)-V^..."l , 



Using these gain values the corresponding updated variance terms are: 



''K*l 









■ X2-^,., 




= xw,., 




- Xc.., j 



Ir^ 

4 



( 3 . 9 ) 



The updated state is written in terms of residuals which are the differences between 
the contact states and predicted states: 
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Residuals - 



Ry 
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Rx 
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RVy 




zVy-llVy 
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zVx-iVx_ 



(3.10) 
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Vy 




Vx 





(3.11) 



S-^KaRy+Kbl RVy 
i*KaRx+Kbl RVx 
\^+Kb2Ry*KcRVy 
Vx+Kb2Rx+KcRVx 

The reduced form equations for the state are included here, but are unnecessary if the 
estimation of the state is to be used. The accuracy of the estimation technique is 
analyzed in the following chapter. 
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IV. MODIFICATIONS TO THE ASSET TRACKER 



The tracker currently used in ASSET makes two major assumptions relating to 
detection modelling: 



• Sensor AOU’s are circles vice ellipses, 

• The velocity variance inherent in the sensor is ignored when making the filtered 
variance calculation for position and velocity contacts. 



Figure 2 shows the geometry of a typical filter operation: 




Figure 2: Geometry of Filter Update Operation. 

The equations which exactly determine the position of the center of the SPA are given 
in (3.12). Given that the simulation knows the true target location and making 
several assumptions detailed below, it is proposed to bypass the Kalman state update 
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step and estimate one possible position of the SPA based on the relationship between 
the SPA center and the true target position. If the true position is distributed 
somewhere in the SPA, then from the point of view of the target, the SPA center is 
likewise distributed about its position. This allows an estimate of a SPA center to be 
generated by drawing a random variable from a distribution with standard deviation 
equal to the square root of the filtered variance in position with a mean centered on 
the target’s true position. This uses the SPA "in reverse" to find a possible position 
for its own center based on the targets true position. Provided the necessary 
assumptions are met, this will provide a reasonable approximation to the filtered track 
over a series of observations. It also allows the variance values to be precomputed 
and drawn from a table vice calculated when needed. These three assumptions are: 

• The mean predicted position and velocity must be very close to the target’s true 
position and velocity so the AOU of the predicted position is concentric with the 
contact AOU, or nearly so. This assumption relates to the motion model in the 
tracker simulated. 

• It is sufficient to model the behavior of detections in the mean, rather than 
explicitly computing them individually. This assumption relates to the 
interaction between the correlator and the tracker. 

• The distribution of filtered states has its mean at the target’s true position and a 
variance equal to the steady-state value of Va (3.10). 

The first assumption sets a requirement for the accuracy of the motion model. 

If the predicted position is consistently offset a considerable distance from the target’s 
true position and the Kalman gain is small, then the center of the filtered position 
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distribution will be drawn off the target’s true position. This can be shown by 
rearranging the terms in Equation (3.12) to yield for a single dimension: 

y = y (1 -Ka) +KaZy+ Kbl {ZVy-Vy) 

The filtered position y approaches the contact position Zy as Ka goes to one and 
moves off to the predicted position S goes to zero. This case must be avoided 
to prevent significant inconsistencies between the distribution of actual filtered 
positions and the estimates generated. Working to keep this relationship is the effect 
shown in (2.5) which acts to push the position toward the observed position as the 
time interval increases for a given observation AOU size and the lOU prediction 
variance gets large compared to it. 

This effect is not easily identifiable in the Kalman gain equations, but increasing 
the time interval increases the size of the predicted AOU and increases Ka for a given 
observation AOU size. This counteracts the tendency of the predicted position to 
recede to the previous filtered position as the time interval increases and keeps the 
required relationship intact for cases where the time interval between observations is 
large. The maximum error will occur when the two AOU’s are nearly the same size 
and the value of Ka is approximately .5-.65. This assumption is also involved in the 
computation of the filtered velocity, where it is assumed that the difference between 
the predicted and observed position multiplied by gain Kb2 is small. 

The second assumption requires acceptance of a lack of consistency between the 
correlator and the tracker models. The ACT-ASSOC module uses the Kalman filter 
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for calculation of the spatial measure of correlation it uses to associate contacts to 
tracks. The effect of using equally representative, but different, implied contact 
locations by the correlator and tracker to do contact association and track updating has 
not been thoroughly examined. The fact that the filtered position produced by this 
method does not relate to the contact position used for track association does not 
appear to be a serious problem, given the Monte Carlo nature of the simulation. The 
draw used by the correlator can be looked at as determining whether the proper track 
assignment is made, a false assignment made or no assignment made. If the contact 
is drawn from the proper distribution, the probability that any single draw results in 
any of these occurrences is the same whether or not the tracker goes on to use the 
contact to update the track chosen. 

The third assumption is again linked to the accuracy of the motion model. The 
result of making this assumption is shown in Figure 3. The first part is essentially 
the effect of assumption one being true. The second part is required because the 
variance in distribution of the actual filtered position is affected by the speed of the 
actual target. For target speeds less than the modelled speed, the actual distribution is 
tighter than the estimated value. The opposite is true if the target speed is faster than 
the model. The implication of this assumption is that the target is exactly performing 
the motion modelled and the tracker does not respond as the actual Kalman niter does 
to deviations from the expected motion. Since the range of velocities the submarine 
targets may have is small, this deviation is typically less than fifteen percent of the 
SPA size. 
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Figure 3: Modified Filter Operation Geometry. 

A. PROPOSED MODIFICATIONS 

Implementation of these changes in the code is a part of a separate effort to port 
ASSET to the Sun UNIX workstation. A listing of the applicable code is contained in 
Appendix B. The update-track object described in Chapter III ia the primary object 
to be modified to implement these changes. Its function can be split between those 
used to precalculate the variance values and those used to estimate the velocity if 
precalculation is desired. This involves computing the three steady state variance 
values for each sensor AOU size, for representative values of the contact interarrival 
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time. Other objects to be changed are mentioned below and included in the Appendix 
B as well. A summary of these modifications include: 



• ioumotion: The three element values of the f matrix (f(0,0), f(0,2) and f(2,2)) 

and the two ^ matrix values and <^>(0,2)) are computed as before, but not 

set in matrix form. The values are used to compute the three element values of 
the predicted variance using Equation (3.6). The three computed values will be 
used to compute the steady-state filtered variance values which are stored in a 
table for future reference. A table of the number of iterations needed to reach 
steady state versus interarrival time is detailed in Appendix C. 

• change-tangent: This object is not required. 

• make-state: The random numbers used to determine the contact’s latitude and 
longitude (from the contactPositionDraw function called by the Detection 
Reporter) can be retained, but as discussed above this seems unnecessary. The 
two values of the R matrix are all that need be calculated by this object. 

• filter: The values computed in ioumotion are used to calculate the three 
elements of the filtered variance using Equation 3.10 for representative values 
of the contact report interarrival time. A graph of the steady state gain Ka 
versus interarrival time is contained in Appendix D. The I^man gains are 
computed in order to find those values and will be used below to set the range 
of operation of the filter. 



This set of routines comprise the pre-processing module and can be separated 
from the rest of MTST so it computes the necessary data at the start of the 
simulation. This requires computing the three variance values for 33 interarrival 
times, giving approximately .03 between succesive table values from .03 to .97. 

This requires 100 values be tabulated for each AOU defined. Retrieving values from 
a data array of perhaps 1500 elements appears to be significantly faster than 
calculating the values individually each time the filter is called. 
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The following routines perform the necessary computations at the time the filter 



operation is performed: 



• center-tangent: This object is modified to call the function 

contactPositionDraw and, using the filtered variance drawn from the table 
computed above, as positional variance choose a center for the SPA. The 
distances involved to not require the use of the spherical earth routine in 
contactPositionDraw. The new latitude and longitude can be calculated using 
1® of latitude=60. 1077nm and 1® of longitude=60. 1077*cos(latitude)nm. The 
error in this estimate is less than one percent out to ranges of 600nm at non- 
polar latitudes. The filtered velocity can be estimated by the following 
equation: 



= Vy^e-’-iX-KSyKSZVy, ^ 

Vx,,^ = Vx,e-’^{\-K3)*K3ZVx, 

• update-track-head: The position and velocity computed in center tangent and 
the new variance values are used as before to update the track-head. The state 
consists of the position latitude and longitude and the corresponding velocity 
components, while the covariance consists of the three steady-state values of the 
filtered variance. 

• update-track-tail: This is modified in the same way as update-track-head to 
account for the new form of the state and covariance. 



The result is a close approximation to the variance values obtained from the 
full implementation and a position that represents the result of some combination of 
predicted position and contact position that was not specifically identified and operated 
on. The distribution of estimated filtered states closely approximates that of actually 
computed states. The specifics of this comparison are detailed below. 



50 



B. ACCURACY OF THE MODIFIED TRACKER 



The assumptions underlying this new procedure introduce deviations from the 
full implementations. As discussed above, the error in the positioning of the SPA 
about the target’s true position error is maximum for Ka approximately .5-. 65. The 
mean value of this error, assuming the worst case of a non-maneuvering target, can 
be calculated using Equation (3.3) to find distance from the old filtered position to the 
new observation position. Since the observations are centered on the true target 
position, the mean of this value can be calculated using the true target position. The 
results are shown in Appendix E for favorable and unfavorable conditions. From 
these results it can be seen that the error is excessive for targets at high speed if the 
time between detections is very long. This quickly drops off however, and for more 
reasonable time intervals of less than one hour, the relative error, even for high speed 
targets, is negligible. 

Assumption three must also be valid for the approximation to be reasonable. 

The validity of the first part of the assumption is shown above. This requires that the 
variance of the distribution of filtered positions be equal to the filtered variance. 
Appendix F presents a comparison of filtered position distributions and filtered AOU 
sizes for various target speeds, computed using the model in the Appendix G. The 
data in Appendix F is the result of modelling 48 hours worth of tracking data or no 
more than 300 observations. The simulated target conducted a random tour consisting 
of ten course changes executed at time intervals with an exponential distribution and a 
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mean of 4 hours. The target’s speed was either 3 knots, 10 knots or 30 knots. The 
standard ASSET lOU parameters /3=.25 and <r=10/3‘'^ were used. The results of 30 
different iterations were averaged to determine the mean values. 

The results indicate that for long time intervals between contacts, and speeds 
near the modeled one, the results are fairly good. Excessive deviations are exhibited 
when the target’s speed is far in excess of the modeled speed. Even in the case 
where the actual target speed was ten knots, the damping of the velocity is apparent in 
the filtered mean speed. When this is combined with the offset error described above, 
errors of up to sixteen percent can result. While this is a large error, the average 
value this error takes through the range of common time intervals between 
observations (.1 hours to 1 hour) and possible contact speeds, is closer to five 
percent. 

One source of this error can be seen by comparing (3.10) and (3.12). While the 
single Kalman gain Ka is used to compute the value of the variance, the actual 
distribution is computed using both Ka and Kbl. For the range of AOU sizes 
associated with submarines and MPA assets (at worst 25nm) leaving the SPA 
unadjusted amounts to an error in standard deviation of the estimated distribution of 
less than Inm. The error is typically conservative, increasing the SPA size by the 
error, and is largest when the time interval between contacts is small. This produces 
errors which, while a significant percentage of the AOU size, are actually quite small. 
A notable exception is the decrease in the standard deviation in situations where the 
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target speed is high, counteracting some of the lOU process’ tendency to 
underestimate the targets speed in this case. 

The failure of the estimate to account for the target’s velocity causes the 
resultant track to be erratic. This effect worsens as the distance traveled between 
observations gets small with respect to the AOU size. The actual filter exhibits 
consistant errors in this case, while the estimate zig-zags wildly around the true target 
track. Since the linearity of the track is not critical to any part of ASSET except 
possibly the correlator, the effect is minimal. For any single point the estimate 
produces, a track could exist which includes it. The random draw combines elements 
from all these hypothetical tracks into a single collection which has the proper 
pointwise relationship to the real track. Thus, while specific combinations of 
observation AOU size and observation interval result in poor performance, others 
result in improved performance. 

The performance of the modified filter is reasonably consistent with the original 
filter operation. A 3-5 nm error in a 25 nm AOU does not significantly impact the 
cueing or search efforts of prosecuting platforms and tracker performance is actually 
improved for cases where the target is transiting, moving at high speeds without 
maneuvering. The errors present in the modified tracker appear to have no negative 
impact on the operation of the rest of the simulation and while introducing moderate 
errors in tracking maneuvering targets, balances that with improved performance 
against non-maneuvering ones. Appendix G contains examples of the original filter’s 
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tracking ability and that of the estimate for a variety of sensor AOU sizes and mean 
interarrival times. 

C. UMITING THE RANGE OF FELTER OPERATION 

An interesting effect apparent from Appendix F, is the limiting nature on the 
filtered position distribution of the contact AOU on the one hand and the predicted 
AOU on the other. This situation stems from the same effect that moves the filtered 
position from the observation position toward the predicted position. This can be 
seen directly by looking at the values of the Kalman gains as a function of range for a 
given time interval between observations. Examples of these relationships are shown 
in Appendix H. As can be seen from these graphs, if the observation AOU radius in 
nautical miles is limited to being less than 150 times the time interval in hours, the 
Ka value remains above the "flat" portion of the curves, approaching zero. 
Observations which arrive at the tracker such that the interval since the last contact is 
too small compared to the AOU size will be filtered using primarily the prediction 
information. The effect of the contact is largely ignored. This is the filter’s way of 
saying that the information value of the contact is not high enough to be considered of 
value in computing the filtered position. 

Such contacts can be ignored as there is little difference between the predicted 
position and variance and the resultant state and variance. A lower limit of Inm on 
the observation is hardwired into the make-state and compute-mean-cov objects. If 
the time between contacts is too short, little is gained unless the AOU is tiny. The 
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Inm limit implies that the minimum time interval between contacts is around .01 
hours to allow the contact data to be assimilated into the track. If the contact makes 
no contribution to the track update, there is no reason to perform the filter operation. 
The effect of this can be seen in the data in Appendix F, where the predicted and 
filtered values are very close for small time intervals and large observation AOUs. 
Thus in dense contact environments, with contacts arriving at the fusion center at 
intervals of less than 30 minutes, any contact based on an AOU larger than 150 times 
the time since the last contact in hours, can be ignored and not filtered. This value of 
150 corresponds to a Kalman Gain limit of approximately .1. The exact value is 
rather arbitrary so long as it not too far into the portion of the curves that change 
quickly with variation in observed AOU size. This could be made a user input to 
limit the resolution of the tracker and speed execution time during scenario 
development and testing, and then reduced to provide more accurate track information 
when the data is actually collected. 

In similar fashion tracks which have not been updated in a long time, do not 
contain enough information to affect contacts that arrive with AOUs less than 5-10 
times the time interval since the last observation. If the value of Ka is very close to 
the "flat" portion approaching one, the predicted value is ignored in favor of the 
contact data. This allows similar saving of filtering operations in sparse track 
conditions with accurate sensors. If the time interval between contacts is expected to 
be greater than one hour and the contact AOUs are small, using the raw contact to 
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update the track is a reasonable approximation if the observation AOU size is smaller 
than five times the time between contacts in hours. 

When these two "gating” techniques are utilized, substantial numbers of filtering 
calls can be eliminated. This can be particularly true in scenarios covering large 
ocean areas with many possible targets. During the initial stages of the campaign, as 
search assets slowly get cues for very quiet enemy tactical platforms, detections will 
not be made until the enemy is at close range. This will result in observations with 
relatively small AOUs coming in at long time intervals. As cues are dispatched and 
friendly forces converge on the enemy, even a modest number of platforms soon 
begin producing contact streams that arrive at rapidly decreasing intervals. Putting 
limits on when the filter is invoked will prevent the simulation from wasting efforts 
computing a SPA that does not really use much of the contact information. 
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V. CONCLUSION 



From a discussion of the construction and use of the Kalman filter based 
Maneuvering Target Statistical Tracker it has been shown that the tracker 
implementation in ASSET is not currently matched well to the types of inputs it 
receives. This results in the calculation of many duplicate values, the performance of 
unnecessary matrix manipulations and the calculation of values in situations when 
little useful information is gained. Improvements to the tracker aimed at decreasing 
the computation time which were discussed include: 



• Use of the equations detailed in (3.3) through (3.12) to compute the actual 
filtered variance and position. 

• Filter the variance of the contact distribution about the target prior to conducting 
a random draw, thus producing a position drawn from a distribution 
approximating that of the actual filtered positions variance. Since the 
observations play no part in calculating this variance, appropriate values for the 
sensor AOUs can be precomputed for a range of expected time intervals. 

• Limit the situations where the filter is actually used to those which will result in 
a meaningful amount of information being extracted. 

• Using planar estimations of the latitude and longitude computations rather than 
perform the conversions on a "spherical earth". 



When taken together these modifications greatly reduce the complexity of the 
calculations required to update a track after an observation. They can also be put 
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under user control so the level of fidelity can be adjusted based on the requirements 
of the architecture and the stage in development of the architecture the analyst is at. 
Areas for future work in this area include: 

• Formulating a faster way of estimating the steady state gains for use in a table 
of pre-calculated filtered variances. 

• Analysis of the filtered position distributions of other types of trackers to 
determine if a better method of approximation can be determined, one which 
better accounts for target velocity. 

• Formulating a simple, recursive estimate of the Kalman gains for on-the-fly 
calculation. An attempt at this by using limiting values as the time interval got 
small yielded very good steady state results, but divergence and chaotic 
behavior for small AOU sizes (< lOnm radius) precluded its use iteratively. 

• An analysis of the algorithms in the correlator to determine if all of its 
algorithms are necessary, or if a simple probability of correct/false/no 
correlation could be substituted. If appropriate, determine what those 
probabilities should be. 

• Modelling how the AOU size should change with respect to range and bearing 
from the searcher. A strict linear bearing error vs range is a simple way of 
solving the problem of constant sensor AOU size. The real situation is not 
quite so simple. This requires computation of the beam sizes and beam 
distribution of a towed array and the effect of TMA techniques on determining 
the position course and speed of the target. This would allow for proper 
calculation and filtering of the sensor’s variance in velocity measurement as 
well. 

The last example opens up a plethora of possibilities for analysis of the parameters 
required for input to ASSET. A compendium of data on everything from 
communication network modelling, to aircraft maintenance data analysis, to platform 
on platform engagement simulation, and so on. Well researched values for the 
present and expected future interactions of platforms and their supporting 
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infrastructures need to be produced if ASSET and simulations like it are to be 
available to the average user. 

ASSET has the potential to be a very effective analysis tool. A great deal of 
work must be expended to provide the necessary data to allow its practical use. In its 
present form it requires so much expert knowledge on so diverse a collection of 
topics, it is doubtful one person could perform meaningful analysis without an 
inordinate amount of time spent in research. Continued evolution in the functionality 
of the simulation will provide increased applicability and accuracy of results. 

Increased streamlining of the computational algorithms will be necessary as this 
occurs to keep it running on a desktop workstation in a reasonable period of time. 
Striving for improvement in performance however, without providing a ready source 
of data will most probably preclude widespread use of the simulation. 
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APPENDIX A: CHARACTERIZATION AND ESTIMATION OF ^ AND Q. 



The following equations represent the elemets in the transition matrix, phi, and the lOU process noise, Q. 
Approximation equations which can be used to more quicly calculate the values are also given. 
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The graphs which follow show the behavior of these functions over various time intervals. The variable "time" 
is equal to the elapsed time from the last contact's time of observation to that of the present contact, in hours. 
The estimated values are computed using the first two significant terms in the series expansion of the exponential 
terms. These estimates are very accurate for interarrival times of two hours or less. 
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APPENDIX B: APPLICABLE SOURCE CODE 



This appendix contains portions of the ASSET source code which would be modified 
to implement the changes described above. The remarks indicate the appropriate 
portions which would be deleted or changed. This is not an all inclusive listing, but 
contains the portions most affected by the modifications. 



ACT4fTST 



Apr 2S, ItM 



1 



OBJECT ACT-MTST 

ACT'KTST is An Object LZSP implementation of MIST, the 
Manueverin? Target Statistical Tracker. 



(setq actmtst Ocindof nil)) 



(defobfun (exist act-mtst) (init-list) 

(usual-exist init-list) 

(have 'dtor (/ pi 180)) 

(have 'alpha 0.25) 

(have ‘sigma (sqrt (/ 100 2))) ; 10 Icnot target 



(defobfun (create-ob ject-linJts act-mtst) (osis) 
(have *ldbm (as)c osis Idbm) ) 

) 



(defobfun (start-new-trac)c act-mtst) (uc trac)c-id) 

(let* ( (mean-cov-list ( compute -mean-covar iance uc) ) 

(mean (first mean-cov-list)) 

(var (second mean-cov-list) ) 

(alt (obu-contact-altitude uc) ) 

(uc-spatial-datA (obu-contact-spatial-data uc)) 

(lat (obu-data-f ield-lat uc-spatial-data) ) 

(Ing (obu-data-field-lng uc-spatial-data) ) 

(ctc-list (list uc)) 
track state-cov 

) 

(setq state-cov (make-obu-state- field 

:ref-lat lat :ref-lng Ing 
: state mean : covariance var)) 

(return-from start-new-track 

(make -obu-t rack :id track-id 

:number-of-contacts 1 
: head-state-covariance state-cov 
: head-contact-id (obu-contact-id uc) 
: head-spatial-data uc-spatial-data 
.‘tail -state-covariance state-cov 
: tail -contact-id (obu-contact-id uc) 
:tail-spatial-data uc-spatial-data 
:altitude alt 
: contacts ctc-list 
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ACT4/TTST 



W«d, Apr 25, 1990 



2 



) 



(defobfun (compute-mean- covariance act-mtst) (uc) 

(let* ( (uc- spatial -data (obu-contact-spatlal-data uc) ) 
(type (obu-data-fleld-type uc-spatlal-data) ) 
(lat (obu-data-fleld-lat uc-spatlal-data)) 
(Ing (obu-data-fleld-lng 
(brg (obu-data-fleld-brg 
(maj (obu-data-fleld-raaj 
(min (obu-data-fleld-mln 
(cse (obu-data-fleld-cse 



uc-spatlal-data) ) 
uc-spatlal-data) ) 
uc-spatlal-data) ) 
uc-spatlal-data) ) 
uc-spatial-data) ) 



(cse-unc (obu-data-fleld-cse-unc uc-spatlal-data) ) 
(spd (obu-data-fleld-spd uc-spatlal-data) ) 

(spd-unc (obu-data-fleld-spd-unc uc-spatlal-data) ) 
(mean (ma)ce-array 4 : Initial -element 0)) 

(var (ma)ce-au:ray * (4 4) : Initial -element 0)) 
stationary smaj smin cosb slnb cosb2 slnb2 



) 

(setq stationary (/ (* sigma sigma) (* 2 alpha))) 

(setf (aref mean 2) (* spd (cos (* dtor (- 90 cse))))) 

(setf (aref mean 3) (* spd (sin (* dtor (- 90 cse))))) 

(setf (aref var 2 2) stationary) 

(setf (aref var 3 3) stationary) 

(setq smaj (max maj 1)) 

(setq smin (max min 1) ) 

(setq cosb (cos (* dtor brg) ) ) 

(setq slnb (sin (* dtor brg))) 

(setq cosb2 (* cosb cosb)) 

(setq slnb2 (* slnb slnb)) 

(setq smaj2 (* 0.25 smaj smaj)) 

(setq smin2 (* 0.25 smin smin)) 

(setf (aref var 0 0) (♦ (* smin2 cosb2) (* smaj2 slnb2))) 

(setf (aref var 0 1) (* (- smaj2 smin2) slnb cosb)) 

(setf (aref var 1 0) (aref var 01)) 

(setf (aref var 1 1) (♦ (• smln2 slnb2) (* smaj2 cosb2))) 

(return-from compute-mean-covarlemce (list mean var) ) 



) 



(defobfun 

(let* 



(update-trac)c act-mtst) (uc tracJc) 
((uc-spatlal-data (obu-contact-spatlal-data uc) ) 



(Id 

(type 

(lat 

(Ing 

(brg 

(maj 

(min 

(cse 



(obu-contact-id uc) ) 

(Obu-data-fleld-type uc-spatial-data) ) 
(obu-data-fleld-lat uc-spatlal-data) ) 



(obu-data-fleld-lng 
( obu-data- f 1 eld-br g 
(obu-dat a - f 1 eld-ma j 
(obu-data-f 1 eld-min 
(obu-data-fleld-cse 



uc-spatial-data) ) 
uc-spatlal-data) ) 
uc-spatlal-data) ) 
uc-sp>atlal-data) ) 
uc-spatlal-data) ) 



(cse-unc (obu-data-fleld-cse-unc uc-spatlal-data) ) 
(spd (obu-data-fleld-spd uc-spatlal-data)) 

(s]xl-unc (obu-data-fleld-spd-unc uc-spatial-data)) 
(obs-time (obu-data-fleld-obs-tlme uc-spatlal-data) ) 
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(tracX- spatial -data (obu-tracX-head-spatlal-data tracJc) ) 

(tall -spatial -data (oPu-tracX-tall-spatlal-data track)) 
(last-update-tlme (obu-data-fleld-obs-tlme track-spatial-data) ) 
(tail -update -time (obu-data-fleld-obs-tlme tall-spatial -data ) ) 
(last-state-cov ( obu-t rack-head-st at e-covajr lance track) ) 
(tail-state-cov (obu-track-tall-state-covarlance track) ) 
(last-ref-lat (obu-state-fleld-ref-lat last-state-cov) ) 
(last-ref-lng (obu-state-fleld-ref-lng last-state-cov) ) 
state-cov-old state-cov-ctc state-cov-new delta-time 
contacts time-old mean-var-llst pos etc ctc-sd count 

) 



(setq delta-time (- obs-time last-update-time)) 



(if (>- delta-time 0) 

(progn 

(setq state-cov-old (change- tangent lat Ing 

(ioumotion delta-time last-state-cov) ) ) 

(setq state-cov-ctc (make-state type lat Ing brg maj min cse cse-unc 
spd spd-unc) ) 

(setq state-cov-new (center-tangent (filter state-cov-old 
state-cov-ctc) ) ) 

(setq track (update-track-head track uc state-cov-new 
uc- spatial -data id) ) 

(setq track (update-track-tail track) ) 

) 

(progn 

(if (or (> obs-time tail-update-time) 

(< (obu-track-number-of-contacts track) 

(ask Idbm contact s- f r ora-head-to-tail) ) ) 

(progn 

(setq contacts (append (obu -track-contacts track) (list uc) ) ) 
(setq contacts (sort contacts #* (lambda (x y) 

(< (obu-data-fleld-obs-tlme (obu-contact-spatlal-data x) ) 
(obu-data-field-obs-time (obu-contact-spatlal-data 



y)))))) 



(if (>- obs-time tall-xjpdate-tlme) 

(progn 

(setq state-cov-old tail-state-cov ) 
(setq time-old tail -update -time) 



contacts) ) ) 

(first contacts) ) ) ) 
(first contacts) ) ) ) 



contacts) ) ) ) 



) 

(progn 

(setq mean-var-llst (compute -mean-covariance (first 

(setq lat (obu-data-field-lat (obu-contact -spatial-data 

(setq Ing (obu-data-fleld-lng (obu-contact-spatlal-data 

(setq state-cov-old (make-obu-state-fleld 
:ref-lat lat 
: ref -Ing Ing 

: state (first mean-var-llst) 

: covariance (second mean-var-list) ) ) 

(setq time-old (obu-data-field-obs-time 

(obu-contact-spatlal-data (first 



67 



ACT-MTST 



W«d. Apr 2S. 1990 



4 



D) 



cse-unc 



(last contacts))) 
contacts) ) ) ) } 



) 

) 



) 

) 



) 

(setq count (min (as)c Idbm contacts-£rom-head-to-tail) 

(- (list-length contacts) 1))J 
(doti^nes (i count nil) 

(setq pos (♦ (obu-trac)t-number-of-contacts trac)c) (- count) 1 



(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 



etc 

ctc-sd 
type 
obs-time 
lat 
Ing 
brg 
maj 
min 
cse 

cse-unc 
spd 

spd-unc 
delta-time (- 
state-cov-old 



(nth 

(obu- 

(obu* 

(obu* 

(obu* 

(obu- 

(obu- 

(obu- 

(obu- 

(obu- 

(obu- 

(obu- 

(obu* 



state-cov-etc 



pos contacts) ) 

>contact-spatial-data etc) ) 

“data- field-type ctc-sd) ) 
•data-field-obs-time ctc-sd) ) 
data-field-lat ctc-sd) ) 

data- fie Id- Ing ctc-sd) ) 

-data-field-brg ctc-sd) ) 

data-field-ma j ctc-sd) ) 

data- fi eld-mi n ctc-sd) ) 

data-field-cse ctc-sd) ) 

data-field-cse-unc ctc-sd) ) 
data- field- spd ctc-sd) ) 

data-field-spd-unc ctc-sd) ) 
obs-time time -old) ) 

(change- tangent lat Ing 

(ioumotion delta-time state-cov-old) ) 

(roa)ce-state type lat Ing brg maj min 



) 

cse 



spd spd-unc) ) 

(setq state-cov-old (center -tangent 

(filter state-cov-old state-cov-ctc) ) ) 
(setq time-old obs-time) 

) 

(setq trac)c (update-trac)t-head trac)c uc state-cov-old 

(obu-contact-spatial-data (first 



(obu-contact-id (first (last 



(setq trac)c (update-trac)c-tail trac)t) ) 



) 



(return-from update-trac)c trac)0 



(defobfun (update-trac)t-head act-mtst) (trac)t uc state-cov spatial-data id) 

(let ((contacts (append (obu-trac)t -contacts trac)c) (list uc) ) ) 

) 

(setq contacts (sort contacts #* (lambda (x y) 

(< (obu-data-field-obs-time (obu-contact-spatial-data x) ) 
(obu-data-field-obs-time (obu-contact-spatial-data 

y)))))) 

(setf (obu-trac)c-number-o£-contacts trac)c) (*► (obu-trac)c-number-of-contacts 
trac)t) 1)) 

(setf (obu-trac)c-head-state-covariance trac)c) state-cov ) 

(setf (obu-trac)c-head-spatial-data trac)c) spatial-data) 
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(setf (obu-trac)c-head-contact-ld track) id ) 

(setf (obu-track-contacts track) contacts ) 

(return-from update-track-head track) 

) 

) 



(defobfun (update-track-tall act-mtst) (track) 

(let* ( (nctcs (obu- track-number -of -contacts track) ) 

(head-sc (obu-track-head-state-covariance track) ) 

(head-sd (obu-track-head-spatlal-data track) ) 

(head-id (obu-track-head-contact-ld track) ) 

(tall-sc (obu-track-tall-state-covarlance track) ) 

(tail-sd (obu-track-tall-spatlal-data track)) 

(tail-ld (obu-track-tall-contact-ld track) ) 

(tall-update-tlme (obu-data-fleld-obs-tlme tall-sd) ) 
(contacts (obu-track-contacts track) ) 

type obs-tlme lat Ing brg maj min cse cse-unc spd spd-unc 
mean-var-llst new-tall pos-new-tail new-tall -sd delta-time 
state-cov-old state-cov-ctc state-cov-new 



y) ) ) ) ) 



) 

(setq contacts (sort contacts I* (lambda (x y) 

(< (obu-data-fleld-obs-tlme (obu-contact-spatlal-data 
(obu-data-fleld-obs-tlme (obu-contact-spatlal-data 



) 

(If (<- nctcs 1) 

(progn (setf (obu-track-tall-state-covarlance track) head-sc) 

(setf (obu-track-tall-spatlal-data track) head-sd) 

(setf (obu-track-tall-contact-ld track) head-id) 

(return-frocn update-track- tall track) 

) 

) 



X)) 



(If (<- nctcs (ask Idbm contacts-from-head-to-tall) ) 

(If (« tall-ld (obu-contact-ld (first contacts) ) ) 

(return-from update- track- tall track) 

(progn 

(setq mean-var-llst (compute-mean-covar lance (first contacts))) 
(setq lat (obu-data-field-lat (obu-contact-spatlal-data (first 

contacts) ) ) ) 

(setq Ing (obu-data-fleld-lng (obu-contact-spatlal-data (first 

contacts) ) ) ) 

(setf (obu-track-tai 1-state-covariance track) (make-obu-state-field 
j ref -lat lat 
:ref-lng Ing 

: state (first mean-var-llst) 

tcovarlance (second mean-var-llst) ) ) 

(setf (obu-track-tall-spatlal-data track) (obu-contact-spatlal-data 
(first contacts) ) ) 

(setf (obu-track-tall-contact-ld track) (obu-contact-id (first 

contacts) ) ) 

(return-from update-track-tall track) 

) 

) 

) 
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(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

(setq 

tail-sc) ) ) 

(setq 

spd-unc) ) 

(setq 

state-cov-ctc) ) 
(setf 
(setf 
(setf 



(setq pos-new-tail (- (obu-trac)c-manb€r-of -contacts track) 

(ask Idbm contact s-f rom-head-to-tail) ) ) 
new-tall (nth pos-new-tail (obu-track-contacts track) ) ) 
new-tail-sd (obu-contact- spatial -data new-tall)) 



type 



(obu-data-fleld-type 



new-tall -sd) ) 



obs-tlme (obu-data-fleld-obs-tlme new-tall-sd) ) 
lat (obu-data-field-lat new-tall-sd) ) 

Ing (obu-data-fleld-lng new-tall-sd) ) 

brg (obu-data-fleld-brg new-tall-sd) ) 

ma j (obu-data-fleld-maj new-tall-sd) ) 

min (obu-data-fleld-mln new-tail-sd) ) 

cse (obu-data-fleld-cse new-tall-sd)) 

cse-unc (obu-data-fleld-cse-unc new-tall-sd) ) 

spd (obu-data-fleld-spd new-tail-sd) ) 

spd-unc (obu-data-fleld-spd-unc new-tall-sd) ) 

delta-time (- obs-tlme tall-update-tlme) ) 

state-cov-old (change-tangent lat Ing (loxmotlon delta-time 

state-cov-ctc (ma)te-state type lat Ing brg maj min cse cse-unc spd 

state-cov-new (center-tangent (filter state-cov-old 
) 

(obu-track-tail-state-covarlance track) state-cov-new) 
(obu-track-tall-spatlal-data track) new-tall-sd ) 
(obu-track-tall-contact-ld track) (obu-contact-ld new-tail) ) 



(prune-contacts-bef ore-tail track) ; Get rid of contacts before tail, 

(return-from update -track-tail track) 



(defofcfun (pnine-contacts-bef ore-tail act-mtst) (track) 

(let ( (nctcs (obu-track-number-of-contacts track) ) 
(contacts (obu-track-contacts track)) 

(max-ctcs (ask Idbm contacts-from-head-to-tail) ) 
tail-position 

) 

(setq tail-position (- nctcs max-ctcs)) 

(if (<- tail-position 0) 

(return-from prune-contacts-before-tail nil) 

) 

(dotimes (1 tail-position nil) 

(setq contacts (remove-element contacts 0)) 

) 

(setf (obu-track-mmber-of -contacts track) max-ctcs) 
(setf (obu-track-contacts track) contacts) 

) 

) 
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ACT44TST 



W«d. Apr 25, 1M0 



7 



(let ( (lat (obu-state-fleld-ref-lat state-cov) ) 

(Ing (obu-state-field-ref-lng state-cov) ) 

(mean (obu-state-field-state state-cov) ) 

(var (obu- St ate- field-covariance state-cov) ) 

motion-matrices phi f new-mean new-var 
vartmp phitrn 

) 

(if (- time 0) 

(return-from ioumotion state-cov) 

) 

(setq motion-matrices (calc-mot -mat time) ) 

(setq phi (first motion-matrices)) 

(setq f (second motion-matrices)) 

(setq new-mean (matrix-multiply phi mean)) 

(setq vartmp (matrix-multiply phi var )) 

(setq phitrn (matrix-transpose phi)) 

(setq new-var (matrix-add (matrix-multiply vartmp phitrn) f)) 

(setq phi new-var) 

(setq phitrn (matrix-transpose new-var)) 

(setq new-var (matrix-scalar-roultiply 0.5 (matrix-add phi phitrn))) 
(return-from ioumotion (ma)ce-obu- state- fie Id 
:ref-lat lat 
:ref-lng Ing 
: state new-mean 
: covariance new-var) 



(defobfun (calc-mot -mat act-mtst) (time) 

(let* ((invalpha (/ 1 alpha)) 

(invalpha2 (* invalpha invalpha)) 

(expalphat (exponential (* -1 alpha time))) 
(exp2alphat (* expalphat expalphat)) 

(f (ma)ce-array '(4 4) : initial-element 0)) 
(phi (make-array '(4 4) : initial-element 0) ) 



) 

(if (> time 0) 
(progn 

(setf (aref 
(- (* 
(setf (aref 
(setf (aref 
(* .5 
(setf (aref 
(setf (aref 
(setf (aref 
(setf (aref 
(setf (aref 

) 

(progn 

(setf (aref 
(- (* 
(setf (aref 
(setf (aref 



f 0 0) (* invalpha2 <- time (* invalpha 
2 (- 1 expalphat)) (* .5 (- 1 exp2alphat) ) )) ) ) ) 
f 1 1) (aref f 0 0)) 

f 0 2) (• invalpha2 (•►0.5 I- expalphat) 
exp2alphat) ) ) ) 
f 2 0) (aref f 0 2)) 

f 1 3) (aref f 0 2)) 

f 3 1) (aref f 0 2)) 

f 2 2) (*0.5 invalpha (- 1 exp2alphat ) ) ) 

f 3 3) (aref f 2 2)) 



f 0 0) (* invalpha2 (- (- time) (• Invalpha 
2 (- expalphat 1)) (* .5 (- exp2alphat 1) )))))) 
f 1 1) (aref f 0 0)) 

f 0 2) (* invalpha2 (•► (- 0.5) expalphat 
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(• -1 .5 exp2alphat)))) 

(setf {aref f 2 0) {aref f 0 2) ) 

(serf (aref f 1 3) (aref f 0 2) ) 

(setf (aref f 3 1) (aref f 0 2) ) 

(setf (aref f 2 2) (* 0.5 invalpha (- exp2alphat 1))) 

(setf (aref f 3 3) (aref f 2 2)) 

) 

) 

(setq f (roatrix-scalar-multiply (• sigma sigma) f ) ) 

(setq phi (identity -matrix 4)) 

(setf (aref phi 0 2) (* invalpha (- 1 expalphat))) 

(setf (aref phi 1 3) (aref phi 0 2)) 

(setf (aref phi 2 2) expalphat) 

(setf (aref phi 3 3) (aref phi 2 2)) 

(return- from calc-mot -mat (list phi f ) ) 

) 

) 



(defobfun (exponential act-mtst) (x) 

(if (< X -350) 

(return-from exponential (exp -350)) 

) 

(if (> X 350) 

(return-from exponential (exp 350)) 
(return-from exponential (exp x) ) 

) 

) 



(defobfun (maJce-state act-mtst) (type lat Ing brg maj min cse cse-unc spd spd-unc) 
(let ((mean (maXe-array 4 : initial-element 0)) 

(var (maJce-array * (4 4) : initial -element 0)) 

(stationary (/ (* sigma sigma) (• 2 alpha))) 

(smaj (max maj 1)) (smin (max min 1)) 

) 

(setf (aref mean 2) (* spd (cos (* dtor (- 90 cse))))) 

(setf (aref mean 3) (* spd (sin (* dtor (- 90 cse))))) 

(setq var (maJce-var stationary brg smaj smin) ) 

(return-from ma)ce-state (maJce-obu-state-field 
:ref-lat lat 
:ref-lng Ing 
; state mean 
: covariance var)) 

) 

) 



(defobfun (roaXe-var act-mtst) (stationary brg smaj smin) 
(let* ((var (maXe-array '(4 4) : initial -element 0)) 
(cosb (cos (* dtor brg))) 

(sinb (sin (* dtor brg))) 

(cosb2 (* cosb cosb)) 

(sinb2 (* sinb sinb)) 
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) 



(sniaj2 (* 0.25 smaj smaj) ) 

(smin2 (* 0.25 smin smin) ) 

) 

(serf (aref var 00) (* smin2 cosb2) 

(setf (aref var 0 1) (* (- smaj2 smin2) 

(setf (aref var 1 0) (aref var 0 1)) 

(setf (aref var 1 1) (*♦• (* smin2 sinb2) 

(setf (aref var 2 2) stationary) 

(setf (aref var 3 3) stationary) 
(return-frOTi ma)ce-var var) 



(* sma j2 sinb2) ) ) 
sinb cost) ) 

(* sinaj2 cosb2) ) ) 



(defobfun (filter act-mtst) (state-cov-sol state-cov-ctc) 
(let ((lat (obu-state-field-ref-lat state-cov-sol)) 
(Ing (obu-state-field-ref-lng state-cov-sol)) 
(meanl (obu-state-field-state state-cov-sol)) 

(varl (obu-state-field-covariance state-cov-sol)) 
(mean2 (obu-state-field-state state-cov-ctc) ) 

(var2 (obu-state-field-covariance state-cov-ctc) ) 
(infmatl (ma)ce-array ’(4 4))) 

(infmat2 (maice-array '(4 4))) 

(infvecl (make-array 4)) 

(infvec2 (make-array 4)) 

(tmpmat (make-array '(4 4))) 

(tmpvec (make-array 4)) 
mean var 



) 

(setq infmatl (matrix- invert varl)) 

(setq infvecl (matrix-multiply infmatl meanl)) 
(setq infmat2 (matrix-invert var2)) 

(setq infvec2 (matrix-multiply infmat2 mean2)) 
(setq tn^xnat (matrix-add infmatl infmat2)) 
(setq var (matrix-invert tmpmat)) 

(setq tmpvec (matrix-add infvecl infvec2)) 
(setq mean (matrix-multiply var tmpvec)) 
(retum-from filter (make-obu-state-field 
:ref-lat lat 
:ref-lng Ing 
: state mean 
: covariance var) 

) 



) 

) 



(defobfun (change -tangent act-mtst) (newlat newlng state-cov) 
(let ( (oldlat (obu-state-field-ref-lat state-cov) ) 
(oldlng (obu-state-field-ref-lng state-cov) ) 

(mean (obu-state-field-state state-cov) ) 

(var (obu-state-field-covariance state-cov) ) 

(rot (make-array *(4 4) : initial -element 0)) 
(rottrn (make-array ’(4 4))) 

(newtmp (make-array * (4 4))) 
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) 



(new-ioean (xnaXe-array 4 ) ) 

(new-var (roa)ce-array '(44))) 

latlnglist rngbrgllst cenlat cenlng deltabrg cosb sinb 
rngl rng2 rng3 brgl brg2 brg3 

) 

(setq mgl (sqrt (■► (* (aref maan 0) (aref mean 0)) 

(* (aref mean 1) (aref mean 1))))) 

(setq brgl (arctan (aref mean 1) (aref mean 0))) 

(setq latlnglist (getlatlng oldlat oldlng rngl brgl 0) ) 

(setq cenlat (first latlnglist)) 

(setq cenlng (second latlnglist)) 

(setq brgl (second (getrngbrg oldlat oldlng cenlat cenlng 0) ) ) 

(setq brg2 (second (getrngbrg cenlat cenlng oldlat oldlng 0))) 

(setq rngbrglist (getrngbrg newlat newlng cenlat cenlng 0)) 

(setq rng3 (first rngbrglist) ) 

(setq brg3 (second rngbrglist) ) 

(setq brg4 (second (getrngbrg cenlat cenlng newlat newlng 0))) 

(setq deltabrg (mod (+ (- brg2 brgl) (- brg3 brg4) 360) 360)) 

(setq cosb (cos (* dtor deltabrg))) 

(setq sinb (sin (* dtor deltabrg))) 

Move the mean position: 

(setf (aref new-mean 0) (* rng3 (cos (* dtor (- 90 brg3))))) 

(setf (aref new-mean 1) (* rng3 (sin (* dtor (- 90 brg3))))) 

Rotate the velocity; 

(setf (aref new-mean 2) (*► (* cosb (aref mean 2)) (* sinb (aref mean 3)))) 

(setf (aref new-mean 3) (- (* cosb (aref mean 3)) (* sinb (aref mean 2)))) 



Rotate the covariance: 
(setf (aref rot 0 0) cosb) 
(setf (aref rot 1 1) cosb) 
(setf (aref rot 2 2) cosb) 
(setf (aref rot 3 3) cosb) 
(setf (aref rot 0 1) sinb) 
(setf (aref rot 2 3) sinb) 
(setf (aref rot 10) (- sinb)) 
(setf (aref rot 3 2) (- sinb)) 



(setq newtmp (matrix-multiply rot var) ) 

(setq rottrn (matrix-transpose rot) ) 

(setq new-var (matrix-multiply newtn^ rottm) ) 

(setq rot new-var) 

(setq rottrn (matrix -transpose new-var) ) 

(setq new-var (matrix-scalar-multiply 0.5 (matrix-add rot rottrn))) 
(return-from change -tangent (maJce-obu-state-field 

:ref-lat newlat 

:ref-lng newlng 

: state new-mean 

: covariance new-var) 

) 
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(defobfun (c«nter-tang«nt act-otst) (stat«-cov) 

(let (dat (obu- state- field-ref -lat state-cov)) 

(Ing (obu-state-field-ref-lng state-cov)) 

(mean (obu-state- field-state state-cov) ) 

(var (obu-state-field-covariance state-cov) ) 

(rot (maJce-array * (4 4) : initial -element 0)) 

(rottm (maXe-array * (4 4))) 

(newtmp (roaJce-array '(4 4))) 

(new-mean (maJce-array 4 ) ) 

(new-var (maJce-array '(4 4))) 

latlnglist rngbrglist newlat newlng newrng newbrg 
rngi brgl cosb sinb 

) 

(setq rngl (sqrt (■*’ (* (aref mean 0) (aref swan 0)) 

(* (aref mean l) (aref mean 1))))) 

(if (< mgl 0.01) 

(return- from center -tangent state-cov) 

) 

(setq brgl (arctan (aref mean 1) (aref mean 0))) 

(setq latlnglist (getlatlng lat Ing mgl brgl 0)) 

(setq newlat (first latlnglist)) 

(setq newlng (second latlnglist)) 

(setq rngbrglist (getmgbrg newlat newlng lat Ing 0)) 

(setq newrng (first rngbrglist) ) 

(setq newbrg (mod (■*> (second rngbrglist) 180) 360)) 

(setq deltabrg (- newbrg brgl)) 

(setq cosb (cos (* dtor deltabrg) ) ) 

(setq sinb (sin (* dtor deltabrg))) 

Move the mean position: 

(setf (aref new-mean 0) 0) 

(setf (aref new-mean 1) 0) 

Rotate the velocity: 

(setf (aref new-mean 2) (<«'(* cosb (aref mean 2) ) (* sinb (aref mean 3) )) ) 

(setf (aref new-mean 3) (- (* cosb (aref mean 3)) (* sinb (aref mean 2)))) 

Rotate the covariance: 

(setf (aref rot 0 0) cosb) 

(setf (aref rot 1 1) cosb) 

(setf (aref rot 2 2) cosb) 

(setf (aref rot 3 3) cosb) 

(setf (aref rot 0 1) sinb) 

(setf (aref rot 2 3) sinb) 

(setf (aref rot 1 0) (- sinb)) 

(setf (aref rot 3 2) (- sinb)) 

(setq ne%rtaip (matrix-multiply rot var)) 

(setq rottm (matrix-transpose rot)) 

(setq new-var (matrix-multiply newtmp rottm)) 

(setq rot new-var) 

(setq rottm (matrix-transpose new-var)) 

(setq new-var (matrix -scalar-mult iply C.5 (matrix-add rot rottm))) 
(retum-from center -tangent (ma)ce-obu-state-field 

: ref -lat newlat 

: ref -Ing newlng 
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) 



: covariance new^var) 



(defobfun (arctan act-mtst) (y x) 

{let {a 
) 

(if (and (- y 0) (- x 0)) 

( return- fron arctan 0) 

) 

(if {- y 0) 

(progn (if (< x 0) 

(retum-frore arctan 270) 
(return-froD arctan 90) 



(if (< y 0) 

(setq a (<*> (/ (atan (/ x y)) dtor) 180)) 
(setq a (/ (atan (/ x y)) dtor) ) 

) 

(If (> a 0) 

(return-froB arctan a) 

(return-frcB arctan (^ a 360) ) 

) 

) 

) 



This procedure converts a covariance Matrix (cov) into an 
ellipse. The ellipse is returned in the form: 

( Orientation « Seed-major axis , Seed -minor axis ). 



(defobfun (cov-to-ellipse act-mtst) (cov) 

(let ((a (aref cov 0 0)) 

(b (aref cov 0 1) ) 

(c (aref .cov 1 1) ) 

brg coab sinb cosb2 sinb2 sincos2b aijr smnr 



) 

(if 

) 

(if 



(not (> (- (* a c) <• b b)) 0)) 

(retum-froa cov-to-ellipse (list 0 (expt 10 9) (expt 10 9) ) ) 



(• b 0) 

(if (not (> a O) 

(retum-froB cov-to-ellipse (list 0 (* 2 

(• 2 

(return-fron cov-to-ellipse (list 90 (• 2 

(• 2 



(sqrt (abs c) ) ) 
(sqrt (abs a) ) ) ) ) 
(sqrt (abs a) ) ) 
(sqrt (abs c) ) ) ) ) 



) 

) 

(if (> b 0) 

(if (- a c) 

(retum-frcB cov-to-ellipse (list 45 (• 2 (sqrt (♦ a b))) 

(• 2 (sqrt (- a b) ) ) ) ) 

) 

) 

(If (< b 0) 

(if (- a c) 

(retiim-frcm, cov-to-ellipse (list 135 (• 2 (sqrt (- a b))) 

(• 2 (sqrt (♦ a b) ) ) ) ) 

) 

) 

(setq brq (/ (arctan (- c a) (• 2 b)) 2)) 

(setq sinb (sin (* dtor brq))) 

(setq cosb (cos (* dtor brq))) 

(setq sinb2 (* sinb sinb)) 

(setq cosb2 (* cosb cosb) ) 

(setq slncos2b (* 2 b sinb cosb)) 

(setq smjr (• 2 (sqrt (♦ (* a sinb2) sincos2b (• c cosb2))))) 

(setq smnr (• 2 (sqrt (♦ (• a cosb2) (• -1 slncos2b) (• c sinb2))))) 
(return-from cov-to-ellipse (list brq smjr annr) ) 

) 

) 



76 



Mitrlx functions 



Wsd« ltey3,lOM 






lisp matrix manipulation routines 
4/28/89— s.c. lent 

these routines operate on three (3) 'standard type' lisp objects which are 
subsets of lisp object types: 

1. scalars, i.e., lisp type 'number* 

2. vectors, l.e., lisp type ' simple -vector ’ with 

length > 1 

3. matrices, l.e., lisp typ>e 'array' with 

rank - 2 and 
each dimension > 1 

all routines accept input, and produce output of these types 

unacceptable inputs produce 'NIL* as output, there are two (2) cases: 

1. 'non-standard* inputs 

2. 'standard' but mismatched inputs, e.g., 
the inverse of matrix (m x n), where m o n. 



routines consist of: 

1. rax -mult 

2 . 

3. 

4. 

5. 

6 . 



mx-add 

sc-mx-mult 

mx-trans 

mx-ident 

mx-lnverse 



these are built using the following 'sub-routines': 



1. matrixp 

2. array-row-el 
array-col-el 
array-row-dim 
array-col -dim 
can -fflx -mult 
can-fflx-add 
sqr-fflx 
mx-factor 
mx-subst 



9. 

10 . 



(defun matrix-fflultiply (a b) 

(if (not (can-mx-mult a b) ) 

(return-from matrix-multiply NIL) 

) 

(let ((c) (ij-list)) 

(setq c (make-array (remove 1 (list (array- row-dim a) (array-col-dim b) ) ) 
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) 

(do ((Indexl 0 (■*■ indexl 1))) 

((> indexl (- (axray-col-dim b) 1))) 

(if (> (^u:^ay-col-dlIn b) 1) (setq ij-list (list indexl)) 

(setq ij-list 0) 

) 

(do ((index2 0 (>«• index2 1))) 

((> index2 (- (^u^^ay-^ow-dim a) 1))) 

(if (> (eurray-row-diin a) 1) (setq ij-list (cons index2 ij-list)) 

) 

(setf (apply f'auref c ij-list) 0) 

(do ((index3 0 (♦ index3 1))) 

( (> index3 (- (array-dlnension b 0) 1))) 

(setf (apply #*aref c ij-list) (•♦• (apply I'axef c ij-list) 

(* (axray-col-el a (list index2 

index 3) ) 

(array-row-el b (list index3 

indexl ) ) 



(if (> (array-row-dim a) 1) (setq ij-list (cdr ij-list)) 
) 

) 

) 

(if (equal (array-dimensions c) () ) 

(aref c) 
c 

) 

) 

) 



(defun matrix-scaleur-multiply (sc mx) 

(if (not (and (numberp sc) (matrlxp mx) ) ) 

(return-frcro matrix-scalar-mult iply NIL) 

) 

(let ( (ij-list) (mx-new (ma)ce-array ( ^u^r ay-dimens i ons mx) )) ) 

(do ((indexl 0 (+ Indexl 1))) 

( (> indexl (- (^u^ray-col-dim mx) 1) ) ) 

(if (> ( aurr ay-col -dim mx) 1) (setq ij-list (list indexl)) 

(setq ij-list 0) 

) 

(do ((index2 0 index2 1))) 

((> index2 (- (array-dimension mx 0) 1))) 

(if (> (array-dimension mx 0) 1) (setq ij-list (cons index2 ij-list)) 
) 

(setf (apply #*euref mx-new ij-list) (* (apply #*auref mx ij-list) sc)) 
(setq ij-list (cdr ij-list)) 

) 

) 

mx-new 

) 

) 
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(defun matrix'transpose (mx) 

(If (not (matrlxp mx) ) 

(return-from matrix-transpose NIL) 



) 

(let 

(do 



((ij-list) (iRx-new (oaJce-array (reverse (array-dinensions mx) ) ) ) ) 

((indexl 0 indexl 1))) 

( (> Indexl (- (array-col-dim mx) 1))) 

(If (> (array-col -dim mx) 1) (setq Ij-llst (list Indexl)) 

(setq Ij-llst ()) 

) 

(do ((lndex2 0 (♦ lndex2 I))) 

( (> index2 (- (array-dimension mx 0) 1))) 

(if (> (array-dimension mx 0) 1) (setq Ij-list (cons lndex2 Ij-llst)) 



) 

(setf (apply faref mx-new (reverse Ij-list)) 
(setq ij-list (cdr ij-llst)) 



(apply I’aref mx ij-list)) 



(defun matrix-add (mxl mx2) 

(If (not (can-mx-add mxl mx2)) 

(ret urn- from matrix -add NIL) 

) 

(let ( (Ij-llst) (mx-new (ma)ce-array (array-dimensions mxl) )) ) 

(do ((indexl 0 (<*’ indexl 1))) 

((> Indexl (- (array-col-dim mxl) 1))) 

(If (> (array-col-dim mxl) 1) (setq ij-list (list indexl)) 

(setq ij-list ()) 

) 

(do ((index2 0 (<^ index2 1))) 

((> lndex2 (- (array-dimension mxl 0) 1))) 

(if (> (array-dimension mxl 0) 1) (setq ij-llst (cons lndex2 ij-list)) 
) 

(setf (apply t'aref mx-new ij-llst) (-► (apply I'aref mxl Ij-llst) 

(apply f'aref mx2 Ij-list))) 

(setq ij-llst (cdr ij-list)) 

) 

) 

mx-new 

) 

) 



(defun identity-matrix (m) 

(If (not (and (integerp m) (> m 1))) 

(return-from identity-matrix NIL) 

) 

(let ( (ij-llst) (mx-new (maXe-array (list mm)))) 
(do ((indexl 0 (♦ indexl 1))) 
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((> ind«xl (- m 1))) 

(setq ij-list (list index!)) 

(do (dndex2 0 (<»■ lndex2 1))) 

( (> index2 (- m 1) ) ) 

(setq Ij-llst (cons indax2 ij-list)) 

(setf (apply #'araf mx-new ij-list) (If (* index! index2) ! 0)) 
(setq ij-list (cdr ij-list)) 

) 

) 

mx-new 

) 



(defun matrix-invert (a) 

(if (not (sqr-mx a)) 

(return-from matrix-invert NIL) 

) 

(let* ( (n (array-diiwnsion a 0)) 

(temp (mx-f actor a n) ) 

(w (car tamp) ) 

(ipivot (cadr temp) ) 

) 

(if (« 0 (caddr temp) ) 

(return-from matrix-invert NIL) 

) 

(mx-subst w n ipivot) 

) 

) 



(defun matrixp (a) 

(if (arrayp a) 

(cond ( (simple-vector-p a ) 

(if (> (array-dimension a 0) 0) 

T 

NIL 

) 

) 

( (equal (array-rank a) 2) 

(if (and (> (array-dimension a 0) 0) 
(> (array-dimension a 1) 0) 

) 

T 

NIL 

) 



) 

(T NIL) 



) 

NIL 
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(defun erray-row-el (a 1) 

(apply #'aref a (if (• (array-ranJc a) 1) 
(list (car i)) 
i 



(defun array-col-el (a i) 

(apply f'aref a (if (• (array-ranX a) 1) 
(cdr i) 
i 



(defun array -row-dim (a) 

(if (- (array-ranX a) 1) 

1 

(array-dimension a 0) 

) 

) 



(defun array-col -dim (a) 

(if (- (array-ranX a) 1) 

1 

(array-dimension a (- (array-ranX a) 1)) 

) 

) 



(defun can-fflx-fflult (a b) 

(if (and (matrixp a) (matrixp b) ) 

(if (- (array-dimension a (- (array-ranX a) 
(array -dimens ion b 0) 

) 

T 

tJIL 

) 

NIL 



) 

) 



D) 



(defun can-mx-add (a b) 

(if (and (matrixp a) (matrixp b) ) 

(if (and (- (array-dimension a 0) 
(array -dimens ion b 0) 

) 



81 



Itatrtx functions 



Wsd. Miy 3, 1»M 



e 



(- (array-dimension a (- (array-rank a) 1) ) 
(array-dimension b (- (array-rank b) 1) ) 

) 

) 

T 

NIL 

) 

NIL 

) 

) 



(defun sqr-mx (a) 

(if (matrixp a) 

(if (- (array-rank a) 2) 

(if (- (array-dimension a 0) (array-dimension a D) 
T 

NIL 

) 

) 

NIL 

) 

) 



(defun no<-factor (mx n) 

(let ( (awikod) (CO Imax) (ratio) (rowmax) (temp) (istar) (iflag 1) 

(ipivot (make-array n) ) 

(d (make-array n>) 

(w (make-array (array-dimensions tnx) ) ) 

) 

(if (<- n 0) 

(return-from mx-factor nil) 

) 



(do ((i 0 (♦!!)) 

(ro%cnax 0 0) 

) 

({- i n>) 

(setf (aref ipivot 1) i) 

(do ((j 0 (♦ 3 1))) 

((- 3 n>) 

(setf (aref « i 3> (aref mx i 3>) 

(setq rowmax (nuuc rowmax (abs (aref w i 3 )))) 

) 

(when (• rowmax 0) 

(setq iflag 0) 

(setq rowmax 1) 

) 

(setf (aref d i) rowmax) 



(do ((k 0 (♦ k 1))) 

((- k (- n 1))) 

(setq colmax (abs (/ (aref w k k) (aref d k) ) ) ) 
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(s«tq istar k) 

(do (( 1 (♦ k 1 ) (♦ t 1 ))) 

( (- 1 n) ) 

(s«tq awikod (abs (/ (araf w i k) (arcf d 1)))) 

(whan (> awikod coloax) 

(satq colinax awikod) 

(satq istar 1) 

) 

) 

(If (• coinvax 0) 

(satq if lag 0) 

(progn 

(if (> istar k) 

(progn 

(satq iflag (* iflag)) 

(satq i (araf ipivot istar) ) 

(satf (araf ipivot istar) (araf ipivot k)) 

(satf (araf ipivot k) i) 

(satq taop (araf d istar) ) 

(satf (araf d istar) (araf d k) ) 

(satf (araf d k) taop) 

(do (() 0 (♦ 3 1))) 

( (- j n) ) 

(satq tamp (araf w istar j)) 

(satf (araf w istar j) (araf w k j)) 

(satf (araf w k j) tamp) 

) 

) 

) 

(do ((i (♦ k 1) (♦ i 1))) 

((-in)) 

(satf (araf w i k) (/ (araf w i k) (araf w k k))) 

(satq- ratio (araf w i k) ) 

(do (( j (♦ k 1) (♦ 3D)) 

((- 3 n)) 

(satf (araf w i 3) (- (araf w i 3) (* ratio (araf w k 

3)))) 

) 



(if (- (araf w (- n 1) (- n D) 0) 
(satq iflag 0) 

) 

(list w ipivot iflag) 



(dafun mx-subst (w n ipivot) 

(lat ( 

(b (maka-array n :initial*alacMnt 0)) 

(ainv (maka-array (list n n) ) ) 

(ip) 



1 ) (- 



(do ((col 0 (♦ col 1))) 
((- col n)) 

(satf (araf b col) 1) 



(satq ip (araf ipivot 0)) 

(satf (araf ainv 0 col) (araf b ip) ) 



(do ((i 1 (^ i D) 
(s\sn 0 0) 



) 

((- i n>) 

(do ((3 0 (♦ 3 1))) 

(satq s!» (♦ stfB (• (araf w i 3) (araf ainv 3 col)))) 

) 

(satq ip (araf ipivot i) ) 

(satf (araf ainv i col ) (- (araf b ip) sun)) 



(satf (araf ainv (- n 1) col) (/ (araf ainv (- n 1) col) (araf w (- 
n 1 ) ) ) ) 

(do ((1 (- n 2) (-11)) 

( sum 0 0) 

) 

((- 1 -D) 

(do ( ( 3 (♦ i 1) (♦ 3 1) ) > 

( f- 3 n) ) 

(satq sum (♦ sum (• (araf ainv 3 col) (araf w i 3)))) 



satf (araf ainv 1 col) (/ (- (araf ainv 1 col) s\m) (araf w 1 D 



) 



(satf (araf b col) 0) 



) 



Rtiq> txiftog functiofM 



W«d, Apr 2S. ittO 



1 



TRACK Utility Procedures: 



(defxin (TetlatLng (letl Ingl dist brg flag) 

;flag • 0 greet circle celc 
; • 1 rhumbline 

; brg in degrees, 

(let* ( (let2 letl) (lng2 Ingl) (mg (ren (/ dist 60.1077) 360)) 

(cbrg (cosLisp brg)) (letlt letl) (cletlt) (sletl) 

(srng nil) (crng nil) (ding nil) (temp nil) (clet2 nil) 

) 

(if (- letl 90) (setq letlt 89.9)) 

(If (- letl -90) (setq letlt -89.9)) 

(setq cletl (cosLisp letlt) ) 

(if (- fleg 0) ; greet circle celculetion 

(progn (if (> mg 180) 

(progn (setq mg (- 360 mg) ) 

(setq brg (rea (♦ 180 brg) 360) ) 

(setq cbrg (cosLisp brg) ) 

) 

) 

(if (• cletl 0) 

(progn (if (• (sinLisp brg) 1) 

(setq let2 (- letl mg) ) 

(setq let2 (♦ letl mg)) 

) 

(setq lng2 Ingl) 

) 

(progn (setq sletl (sinLisp letlt)) 

(setq smg (sinLisp mg)) 

(setq cmg (cosLisp mg)) 

(setq let2 (- 90 

(ecosLisp (fnAbs (♦ (* cletl smg cbrg) 

(* sletl crng) ) ) ) ) ) 

(setq clet2 (cosLisp let2) ) 

(setq ding (ecosLisp (fnAbs (/ (- cmg (* sletl 

(SinLisp let2))) 
(* cletl clet2) ) ) ) ) 

(if (< (sinLisp brg) 0) 

(setq lng2 (fnCor (- Ingl ding))) 

(setq lng2 (fnCor (♦ Ingl ding))) 

) 



) 

) ;if cletl-0 

) ; progn mg > 180 

) ;if fleg • gc 

(if (• fleg 1) /rhiabline celc 
(progn (if (or (• brg 90) 

(- brg 270)) 

(progn (setq let 2 letlt) 

(if (- brg 270) 

(setq lng2 (fnCor (- Ingl 



(setq lng2 (fnCor 



(/ rng cletl) ) ) ) 
(<*> Ingl 



(/ mg cletl)))) 

) 

) ;end then construct brg«90 or brg«270 
(progn (setq let2 (<*> letlt (* mg cbrg) ) ) 

(if (>• (ebs let2) 90) 

(progn (if (> let2 0) 

(setq let2 90) 

(setq let2 -90) 

) 

(setq lng2 0) 

) 

(progn (setq temp (log (/ (tenLisp (- 4S (* O.S letlt))) 

(tenLisp (- 45 (* 0.5 let2)))))) 
(setq lng2 (fnCor {* Ingl (* GLB.redToDeg temp 

(tenLisp brgj ) ) ) ) 

) 

) 

) ;end else construct brg»90 or brg»270 

) 

) 

) ; if fleg-1 
(list let2 lng2) 

) ; let 
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{zei'jTi getRngBrq (latl Ingl lat2 lng2 flag) 

: flag - 0 graat circla 
- 1 rhviD^llna 

(lat* ((mg 0) (brg 0) (dlat (abs(' latl Iat2))) 

(ding (abs(fnCor(> Ingl lng2)))) (x) (y) (latlt) (clat2 (cosUsp lat2 )) 

) 

(if (>« dlat ding) 0.0001) 

(progn (if (- flag 0) 

(progn (If (- ding 0) 

(progn (if (< latl lat2) 

(satq brg 0) 

(satq brg 180) 

) 

(satq mg dlat) 

) 

(progn (if (* (abs latl) 90) 

(satq latlt (• 89.9 

(/ latl (abs latl) ) ) ) 

(satq latlt latl) 



(cosLisp ding) ) ) ) ) ) 



) 

(satq clatl (cosLisp latlt) ) 

(satq slatl (sinLisp latl)) 

(satq slat2 (sinLisp lat2)) 

(satq rng (acosLisp (fnAbs(> (* slatl slat2) 

(• clatl clat2 

(satq brg (acosLisp (fnAbs(/ (- slat2 

(* slatl (cosLisp 



mg))) 
mg) ) ) ) ) ) 

datalina crossing 
; is this progn nacassary , 



(* clatl (sinLisp 

(if (or (>- lng2 -90) 

(<• Ingl 90)) ;othar than aaat or wast 

(progn (if (< lng2 Ingl) 

(satq brg (- 360 brg) ) 

(progn (if (and (> lng2 90) 

(< ingl -90)) 

(satq brg (- 360 brg) ) ) 

)) 



)) 

)) ;if flag-0 
(if (- flag 1) 
(progn (if (or ( 



(abs latl) 90) 

(- (abs lat2) 90)) ;Horth or South pola 
(progn (satq mg dlat) 

(if (> lat2 latl) 

(satq brg 0) 

(satq brg 180) ) 



(progn (aatq * (log (/ (tanLlsp (- 45 (• 0.5 latl))) 

IP ^ (tanLlsp (- 45 (• 0.5 lat2)))))) 

(satq X (* X GLB.radToOag)) 

(satq y (fnCor (- lng2 Ingl))) 

(satq brg (fnArctan x y) ) 

(if (or (- brg 90) 

(- brg 270)) 

(satq mg (* clat2 ding)) 

(satq mg (abs (/ dlat (cosLisp brg)))) 



) 



)) 



)) 



) ) 



(satq rng {• 60.1077 rng)) 
(list mg brg) 

) ;lat 
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(setq detect ionR«porter Oclndof qener&lPlatfonn) ) 



(defobfun (exist detectionReporter) (Init-llst) 
(usual -exist Init-llst) 



report -type 


' POS+VEL ) 


; POS-pos 


posltional-unc 


10 ) 


; nm 


speed-unc 


3 ) 


kts 


course-unc 


30 ) 


degrees 


expRepDelay 5) 







(have ‘FAR 0.9) 

(have ’ start TracJcp 1) 

(setq dataNameLlst (append dataNaaieLlst 
' (: reports 

(report-type posltional-unc speed-xmc course-unc 
expRepDelay FAR startTrackp) ) ) ) 

(setq dataValuelist (append dataValuelist 
(: reports 

(, report -ti'pe , posit ional-xinc ,speed-unc , course-unc 
, expRepDelay ,FAR , start Trac)cp) )) ) 

(setq datar> 7 >eLlst (append dataTypel.1 st 

'(:reports (data data data data data data data)))) 

(setq dataTextllst (append dataTextLlst 

*(:reports CREPCRT TYPE" "POSITION ERROR" "SPEED ERROR" 
"COURSE ERROR" "Exp Processing Delay" 

"False Alarr. Rate" "Track Initiaticn") ) ) ) 
(setq dataleirpIateText (append dataTeoiplateText 
• (: reports 

(("POS or POS*VEL" "(POS , POS^VTLI") 

("Nautical Miles" "(0 - 1000)") 

("Knots" "(C - 3Cj") 

("Degrees" "(0 - 100]") 

("Hours" "10 - 100)") 

("FA per Day" "(0 - unlimited]") 

("Single Contact to Track" "0 - no, 1 - 



yes") ) ) ) ) 

(let* ( (rt-designators ' (report s-rb )) 
(rr-values 
(rb-names 



) 



(list d‘.rnr.yRBdata) ) 
' (dufflmyRBdata) ) 



(update-rb-llsts rb*designators rb-values rb-names) 



(defobfun (setForStart detectlonReporter) 0 
(usual -setForSt art) 

(have ' corrputedFAR FAR) 
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Otwctlon Ptoport«r 



W«d.ltar 2i,ltf0 



2 



) 



(d«fobfun (9«tR*portD«l4y d«t«ctionReporter) (4option4l targttTyp«) 
(return-froro g«tReportO«lay 

(exponential Draw expRepDelay) 

) 

) 



(defobfun (make -obu- report detectlonReporter) (time target-parms (optional HFDFp) 



( (cse 


(platformStateParms-cse 


target-parms) ) 


(spd 


( p 1 a t f c mS t at e P arm s - s pd 


target -parms) ) 


(lat 


(plat forms tat ePams- lat 


target-parms) ) 


(Ing 


( pi at f ormS tat eParms - 1 ng 


target-parms) ) 


(hgt 


(platformStateParms-hgt 


target-parms) ) 


(targetName 


( p 1 at f 3 zmS t a t eP arms -pi at f ormName 


target-parms) ) 


(cat 


(make-obu-category ipinnedp nil 






:lockedp nil 






: cluster-count 0 




:assoc-count 


0)) 


(sensor-field (make -obu -sensor 





:name (princ-to^string name) 

:s ingle-report -tO'trackp (not (equalp startTrackp 0)))) 
spatial-data-field 



(if (equalp report -type 'POS--VEL) 

(setq cse (mod (♦ cse (- (random (• 2.0 course-unc) ) ccurse-unO) 

360) 

spd (max 0 (<» spd (• (random (* 2.0 speed-uncn speed-unc)) 

)) 



(setq cse nil 
spd nil) 

) 

(multiple-value-setq (lat Ing) (contactPositionDraw lat Ing 
positional-unc) ) 

(setq spatial -data-field (maXe-obu-data-field 



itype 


report -type 


;obs-tiJM 


time 


Hat 


lat 


:lng 


Ing 


:brg 


0 


;maj 


positional-unc 


isiin 


positional-unc 


:CM 


cse 


:cse-unc 


course-unc 


spd 


spd 


: spd-unc 


speed-unc 



) 

) 

(return-from make -obu- report (raake-obu- contact 

:id nil 
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O«t«etton R«pert«r 



W«d. Mir 21, 1M0 



3 



) 



: track*association 
: a«nsor 

:cat«gorization 

:spatlal>data 

:altltud« 

:HFDFp 



tina 

targatNana 

•anaor-flald 

cat 

spat 1 al 'dat a> f i e 1 d 

hgt 

HTDrp 



(dafobfun (make- false-obu- report detectlonReporter) (lat Ing uncertainty ctlme 
taxgetType) 

(let ( (falseTargetState (getFalsetargetState lat Ing uncertainty targetType) ) ) 
(maXe-obu>report crime falseTargetState) 

) 

) 



(defobfun (getFalsetargetState detectlonReporter) 
(muitiple-value-bind 
(lat Ing) 

(contactPositionCraw lat Ing uncertainty) 
(return- from getFalsetargetState 
(maJce-platformStateParms 
:lat lat 
ring Ing 
:hgt 0 

:cse (* (random 1.0) 363) 

:spd 10 

rplatfcrrnName *FA 
: targetType targetType 

) 



) 

) 



) 



(lat Ing uncertainty targetType) 
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StructurM 



THu. Miy 10, IMO 



1 



<def struct coast-event 
time 
object 
procedure 

data 

updateList 



(def struct coast-nessage 
send- time 
receipt-time 
type 
sender 
content 
size 

transmission-path 

transmission-count 

) 



; Time message sent. 

; Time message received at last node. 
; E.g. ' Detect -msg 



; MBs 



(defstruct obu-contact 
id 

receipt-time 
track -association 
sensor 

categorization 

spatial-data 

altitude 

BFDPp 



(defstruct obu-track 
id 

numbe r-of -contact s 
head- St ate-covariance 
head-contact -id 
head- spatial -data 
tail-state-covariance 
tail-contact -id 
tail-spatial -data 
altitude 

(contacts nil :type list) 



(defstruct obu-data-field 
type 
obs-time 
lat 
Ing 
brg 
maj 

min 

cse 

cse-unc 

spd 

spd-unc 



(defstruct obu-state-field 
ref-lat 
ref -Ing 

(State nil :type (array float 4 )) 

(covariance nil :type (array float '(4 4))) 



(defstruct obu-sensor 
name 

single-report -to-trackp 



(defstruct obu-category 
pinnedp 
lockedp 
assoc-count 
cluster-count 
ambiguity-list 
ambiguity-resolution 
ambiguity-resolution-time 
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ProbabllRy functkons 



Fri, Itar 23. 1990 



S 



(defun contactPositionDraw (Xat Xng rng) 

; a draw from a circuXar nonnaX •• rng Is interpreted as a 2 sigma confidence 
; intervaX 

(Xet ( (randoroRange (min (• (/ rng 2) (sqrt (• -2 (Xog (random X.O))))) 

(• 4 mg) ) ) 

(randonAngXe (randomCse) ) 

) 

(return-from contactPositionDraw 
(vaXues^Xist 

(getlatLng Xat Xng randomRange randomAngXe 

•greatCircXe*) ) ) 

) 

) 

; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; : ; ; ; ; ; ; / ; ; ; ; : ; ; 

; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; 

; SPA rUNCnONS 

; : ; ; ; ; ; : ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; / ; ; ; ; ; ; ; ; ; ; ; ; : ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; 

;;;;;;;;; ;;;;;;;;;;;;;; 

(defun expandAOUpos (veX tau timeLate) 

(Xet ( (ratio (/ timeLate tau) ) 

(factor (• 2 pi veX veX tau tau))) 

(* factor (*► ratio -X (exp (* -X ratio)))) 

) 

) 

; ; ; ; ; : ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; 



(defun expandAOUpos VeX (veX tau timeLate) 

(Xet* ((ratio (/ timeLate tau)) 

(f actor X (* 2 pi veX veX tau tau)) 

(factor2 (* 2 (exp (* -X ratio)))) 

(factors (* C.5 (exp (* -2 ratio)))) 

) 

(* factorX (♦ ratio factor2 -X.5 (* -X factorS))) 






(defun getRandomPointInRegion (region) 

(Xet* ( (regionParms (getRegionParms region) ) 

(minLat (region-parms-minLat regionParms) 

(maxLat (region-parms-maxLat regionParms) 

(mining (re^on-parms-minLng regionParms) 

(maxLng (region-parms'-maxlng regionParms) 

(c)cStuff (region-parms-checlcpts regionParms) 

(thePoint nlX) 



) 

) 

) 

) 

) 



) 

(if (> mining maxing) ( set q mining (• mining 360))) 
(do ( (insidep niX) ) 

(insidep) 

(setq thePoint (Xlst 

(♦ mlnXat (* (- maxXat minXat) 



(random X.O))) 



(<»> mining (* (* maxing mining) (random X.O))))) 
(setq insidep (checklf Inside thePoint c)cStuf f ) ) 

) 

(return-from getRandomPointInRegion thePoint) 

) 

) 
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APPENDIX C: ITERATIONS TO ACHIEVE STEADY STATE GAIN VALUES 



The model in this appendix calculates the percent differece between succesive iterated values of Kalman gain K 
These values are tabulated for various ranges of interarrival time and sensor AOU size. 



n = 0..25 A - 50 u = 100 a - .15 c ■ -JiO 

vIq = tl v2q = 0 v3q = r3 02 = !-•[ I - exp[ -a’ Aj] 03 = e*p[-a'Aj 



r3 = — rl = 25-u^ 
1-a 



2 

a r 



ql = — i'l A - 2-' 1 - exp[ - a* A1] - .S*f I - exp[ -a'2- A’l}j2| = — 1 *[ .5 - exp[ -a* A] -f[ .5-expf -a’2' A]]]j3 - .5- — ’f 1 - exp[ -a'2* A] 

o, I® ■' ' " if a ' ® ■ 

[ I i vl„ +2-02-v2p -l{02f-v3p -Iql |•[[^3f♦v3^ -hi3] -|vlp +2- 02- v2„ -t[ 0zf -vap -hil|T3-[[ v2„ -102-v3p]-03 -h)2 i ■ 



''•n4ll 



v2, 



n -H 



I 



[ vl^ +2-<^2 -v 2„ 4 -h,! 4 t1 ]•[ [ v3^ -K,3 -»t3]-[[v2„ 4i^2-v3„]*<^3 



[f v2jj -h^2*v3jj]*<^3 -Ki2 j-rl-| 



vl^ 4-2*i^2-v2„ i ^2f -v3„ Ml ]•[[ -v3„ -h,3 « j -[[ v2„ -N^2-v3„ ]-<^3 M2 f 





1 

L 


1" 


„+2-02-v2„ 




v3„ -Iql -iTl ]• 




v3„-lt,3 -lr3]- 


[[v2^402-v3j]- 03 -hi2|^ 








kl,-kl,. 1 












k'„ - 


7T ■ 


= 1 ..5 Diff; = 


1 It 

k't 


100 










Sensor AOU 


= Inm 














A =1 


•lO'^ • 


A =0.01 0 


A =0.05 


■ 


A =0.1 I 


1 


A =0.5 ■ 


A =1 • 


Diff; 


■ 


Diff; . 


Difi; 


■ 


dir; 


■ 


dir; 


0 Diff; ■ 



-99.980i»] 


-49.944635 


TTWT] 


24 91273^ 


-19.892447 


- 16.539079 


-14.138292 


- 12.332923 


- 10.924554 


-9 794141 


-8.845931 


-8 6894191 


-7.429679' 


6.8617P 


' 6.367239 


T93BTT 


- 5.54706 


-5.202706! 


-4.893016' 


■4.6128391 


-4.3580071 


-4 12511 


-3.911334 


'-3.‘71433i 


1-3.532138 



■98.0376H 


-45.80426^ 


-27.592024 


18.19873* 


-IJ.JSSW 


-8.932984 


-6.5J5M5 


-4 903215 


-3.800901 


-3.04773 


-1.429217 


-2. 167^54 


-1.909555 


1.718W7 


-1.57099 


- 1 .449334 


-1.343167 


T 144689 


-I.15J855* 


- 1.0625081 


0.973753? 


0.886509* 


-0.801183' 


1-0.718437' 



66.57563 

Trmm 

^;5 OT b T 5l 



-6.6^^ I 
TT5^ 



■3.855277I 

-J;51M14 



3^6929! 



inmsr 



33.15482] 

Tmm 



9.444393 

7.(34 1604 



■'1.284444 

•0.298493 



-0.035621 

^0.004605 



1.894728 

4.479219 



inm^ 

0.008118 



-5.581487-10 



0.469314 

■0:82I439- 



'0.-03142T- 



8.463799*10 



-2.082199*10 



FI 



-6.(308734' 



o.6io4l2 



0.003814 



■0.004216j 

-0.004 



-6.004474'- 

"0.003487' 

-0.002352 
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Sensor AOU = lOnm 



A =0.01 



A =0.05 O 



A =0.1 



A =0.5 . 



A =1 



A =5 



Difi; 



99 979987 


-49.955028 


-33.266776 


-24.912704 




-16.53869^ 


14.1375271 


^ I 2 . 33 I 6 I 2 ] 


-i0.92250^ 


-9.791141 1 


-ft.il6t754l 


- 8.083836 [ 


■^.4554iF, 


-6.852556 


-6 355922 ! 


“5^.918804 


-5.530716 


-5.18351 


-7.870759 


-4.58733 


-4.329072 


-4.0925971 


■^.8751 IT 


-3.^74191 ■ 


-3.488188 



Din; 



■ 99 5004S7 


^ 48.8945891 


-31.73098' 


1- 22,95 12 OII 


f-1 7 5465691 


13.849295 


- 11.145522 


-9.078576 


-7.450049 


-6.14M37 


-5;(^'0i7 


-4.192852 


^3.465175 


-2.860974 




-i ^4058 


1.593814 


-1.30588 


^.667456 


“0704^1 


6.708084 


-0.574577 


-0.465089 


■=0.375535 


-0.302477 



Diff; 



Difi; 



1-98. 0236931 


- 45.802954 


-27i^5l5a 


1- 18 1651751 


!- 12.438541 


r-'8.7>»27! 


-6.118373 • 


-4.328909 


-3.065327 


-2.16778 




-1.074192 


^-7^1363 


-0.522938 


-0 362028 


-6.24536 


-0.170673 


-0.116232 


■^75157574?“ 


-6.6^3054 



65.885691 

'“12.83yi^ 

rriMTTg- 

1 - 0 . 413997 ^ 

!-0.oolT575! 

- 0.0017^6 ^ 



Difi; 


0 C 


-32.043382 


_ 


-6.917853 


- 


-3.044708 


- 


-0.490318 




-0.027738 




- 



Dift^ 

1.873284 

0:74939T 



-0.01350f 



-1.586052-10 



1.635119-10 



Sensor AOU = lOOnm 



A -0.1 0 


A =0.5 0 


A =1 . 


A =5 ■ 


A =10 0 


A =50 


Difq . 


Difi; . 


Difi; • 


Din; . 


DiflJ • 


Difi; 



99.979843 


-49.9550o4 


-il266816 


ttttottI 


-19.89094 


- 16.535544 


hTTiTin 


r lTi22629 


-10.9096^^ 


-9 774059 


-046li^3 


-8.0576071 


- 7 JV1589 


-6.817245 


-6.31636 : 


-5 8752^1' 


-5.483573! 


5.13315 


-4 8175l6f 


■=T5ir^ 


-4.271329 


'4 033201 


-3.8I4445I 


-3 61272 


-3 42606 . 



-99.484887 


1-48.896967 


-Tl .7380271 


-22 9230181 


•17 45ii7J 


- 1 3.674001 


-10.895156 


-8.76^84 


-7.102159 


- 5.774855 


-4.70748 


J 843648 


3 14164V 


- 2.569625 


-2.102692 


mil 64 


-T.46562T 


r.“ra674^ 


-0.944669 


'-0.773565 


0.63347 


» -6.5187^(i 


!-6.4248t^ 


-6.347893 


-0.284898 



F 97.923273 


r 45.855825 


27 649454 


1-18.018772 


FTTWTT?? 


1-8. 113983 


-5.470752 


-3.68^715 


-2.4*0428 


-1.^775^ 


l-T. 1265 ^ 5 ^ 




-0.5U5316 


-0.3392141 


■-■?J,2'57S5!r 


-0.152814 


r-T);i'65559 


*0.068829 


-6.64^161* 





-65.624462 


- 


- I 4 . 771 W 




-3.1 3(3756 




- 0.559593 




-6.09i7i5 


- 



36.291752 

-4:450393' 



■0.269804 

TOJimr 



-5.467805-10 






6.493503 

0.141054" 



2.531692*10 



r4 



4.543601 - 10 



-8.154349-10 
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APPENDIX D: KALMAN GAIN VERSUS INTERARRTVAL TIME 



This appendix shows the relationship between the Kalman gain and the interarrival time. This 
information can be used to select a range of time values to tabulate preprocessed variance data. 

2 

n = 0..2S A = 10 u = SO a = .23 = ^/M 2 r3 = — 

rl = ,25'u 2* a 



v2o = 0 



v3o = r3 



^2 = 1 -exp[-a’A]] 

a 



o 



A-[i*[[2.[l-e,p[-o-A])-.5.[l-.,p[-a-2.A]]]]]] 



^3 = cxp[-a*A] 2 

q3 = .5* — •[ 1 - cxp[ A]1 
a 

2 

q2 = .5 - exp[-a* a] .5-cxp[-a*2-A]]] 

a 






I [ vl„ +2- 02- v2„ Hf A2 f • V3^ 4ql ]•[ [ 43 f • V3„ I H 


( vl„ -«• A2 -v 2„ -t( «2f -v3„ -Kjl ]• 


■ r3 - [ [ v2„ -1^2 • v3„ ]• «3 -Kl2 f ] 


1 

,3.1 -ll”'' 


^l„+2•«2•v2„^f«2f•v3„ -H,l -kl] 

' <63 *4fi^ 1 • rl • 


-[[<^3f-v3„-h,3-k3] 

r3 


-[[v2„-H^2-v3„]-<^3-h,2]^ 


ri ^ ■ ..... . . 

[vln +2*^2 -v2„ -1(^2] • 
*42- v3j| ]• 43 -iifl -l| vl^ -1-2- 42- v2„ 


v3^-hil -kl ]•][ 03f-v3^ -hi3 -ks] 
-If ^2f -v3„ -hil ]•[[ <^3f -v3„ -hp 


-[[v2„-I^2-v 3„]-^3 -ki2]^ 
]-k»*[[*3f-v3„-Kp]] 


1 


[vl„■^2•^2•v2„^(«2f•y3„-K,l -kl] 


•[[^3f -v3„ -tij3 -k3 


-[rv2„-l^2-v3„]-.^3-hi2]^ 



kl 



n 





KDATA = READPRN(KAlnian) 



Sensor AOU =lnm 




time = KDATA 



Ug - APPENDPRN(Kelm«n) = [A u kl 25 lOejj k2b25 >c325]e 

Kl = KDATA n = 0 ..1*4 KDATA*® 
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KDATA READPRN(K,l™n5) . ^DATA > K1 = KDATA > n = 0 ..U« KDATA <> > ] 

Sensor AOU =5nm 




KDATA = KEADPRN(KiliiunlO) 

Sensor AOU =10nm 



lime = KDATA ^ K1 = KDATA ^ 



n - O..Un[ KDATA 




KDATA = READPRN(Kjlm«n25) 0 > O r o>l 

Ume = KDATA Kl =KDATA^^ n - 0 ..Uell KDATA^ ^ J 

Sensor AOU =25nm 




limc^ 

n 
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KDATA READPRN(Kalman50) 

Sensor AOU =50nm 



time ^ KDATA K1 - KDATA 



n = 0 . . Ia8l[ KDATA 



0>1 




KDATA ^ READPRN(KalmlOO) 

Sensor AOU =100nm 



time = KDATA ^ ^ K1 = KDATA ^ ^ 



n = O..U«[kDATA^^] 
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APPENDIX E: OFFSET ERROR OF FILTER POSITION DISTRIBUTION 



This appendix provides indications of how far from the target’s true position the 
filtered position is displaced by the lOU process adjustment of velocity to account for 
the possibility of a manuever. This assumes the target is actually moving with 
constant course and speed. The first graph cover’s the entire range from Inm to 
lOOOnm while succeding graphs show only the neighborhood of the maximum value 



Offset Error for 10 kt Target. 

Interarrival Time: 5 hours. 




96 



Percent Error Compared to Filtered AOU. 



Offset Error for 30 kt Target. 

Interarrival Time: 5 hours. 




Offset Error for 3 kt Target. 

Interarrival Time: 1 hour. 
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Percent Error Compared to Filtered AOU. Percent Error Compared to Filtered AOU. 



Offset Error for 10 kt Target. 

Interarrival Time: 1 hour. 




Offset Error for 30 kt Target. 

Interarrival Time: 1 hour. 
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Percent Error Compared to Filtered AOU. Compared to Filtered AOU. 



Offset Error for 10 kt Target. 

Interarrival Time: 0.1 hour. 




Offset Error for 30 kt Target. 

Interarrival Time: 0.1 hour. 
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APPENDIX F; ERROR BETWEEN FILTERED AND ESTIMATED SPA SIZE 
This appendix shows the difference in standard deviation between the actual 
filtered position distribution and the estimated distribution. The data tabulated shows 
more complete data for the cases modeled. This includes the observed AOU 
(Obs.AOU) and the predicted AOU (PRED AO); the mean value of the standard 
deviation calculated for 30 runs of 48 hours of 300 observations for the position 
predicted by the lOU model, the contacts position, and the filtered position; as well as 
the actual mean speed of the contact over the 30 runs and the mean filtered value. 



Time=4 Hours, Target Speed=lOkts. 

PifwnMara: MplM-.25. V>10 kts. 




100 



D«vta(lon in nm 



30 



Time=i Hour, Target Speed=3kts. 

Pararrwterv: Alpha- .25. V- 1 0 kta. 




Time=1 Hour, Target Speed =10kts. 

Paramatera: Alpha«.2S. V-IOktt. 
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35 



Time=1 Hour. Target Speed=30kts. 

PwwMMra. Alpha-.2S. V-10 kM. 
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Data for on lOU process motion model beta=.25 and sigma based on 10 knot average speed 
Time interval between observations of 4 hours: 



Standard Deviation of 
Prediction Contact 
mean mean 


error in nautical miles: 
Filter 

mean SPA 


%Error 


Obs. AOU 


Pred. AO 


Speed in knots. 
Contact Filtered 
mean mean 


14.941 


11.957 


11.032 


11.499 


-4.06122 


25 


30.684 


2.959 


3.044 


20.041 


19.071 


16.716 


17.18 


-2.70081 


40 


34.216 


2,975 


3.145 


22.194 


23.148 


19.693 


20.594 


-4.37506 


50 


36.52 


3.078 


3.078 


33.145 


43.599 


32.113 


35.113 


-8.54384 


100 


47.234 


3.02 


2.635 


52.749 


81.301 


53.04 


59.109 


-10.2675 


200 


67,285 


2.92 


2,118 


92.721 


160.022 


93.447 


103.544 


-9.75141 


400 


108.788 


2.992 


1,881 


149.547 


245.976 


151.585 


148.352 


2.179276 


600 


152.841 


3.061 


1.895 


21.938 


11.925 


11.197 


11.499 


-2.62632 


25 


30.684 


10.133 


7.482 


27.577 


19.357 


17.561 


17.18 


2.217695 


40 


34.216 


10.026 


7.087 


27.27 


22.54 


19.703 


20.594 


-4.3265 


50 


36,52 


9.997 


6.81 


40.463 


43.917 


34.288 


35.113 


-2.34956 


100 


47.234 


9.952 


6.369 


57.402 


84.078 


54.921 


59.109 


-7.08522 


200 


67.285 


9.943 


5.999 


95.207 


154.341 


95.083 


103.544 


-8,17141 


400 


108.788 


10.307 


5,925 


146.205 


243.202 


147.258 


148.352 


-0.73744 


600 


15Z841 


10.076 


5.958 


56.41 


13.4 


13.682 


11.499 


18.98426 


25 


30.684 


29.933 


21.703 


59.391 


21.688 


22.186 


17.18 


29.13853 


40 


34,216 


29.931 


20.036 


63.26 


26.691 


26.537 


20.594 


28.85792 


50 


36,52 


29.962 


20.124 


78.533 


47.705 


47.533 


35.113 


35.37151 


100 


47.234 


30.019 


18.783 


101.683 


89.644 


78.28 


59.109 


32.4333 


200 


67.285 


30.001 


18.033 


133.696 


169.918 


121.254 


103.544 


17.10384 


400 


108,788 


29.908 


17.635 


169.943 


233.559 


160.386 


148.352 


8.111788 


600 


152.841 


30,072 


17.556 


Time inten/al between observations of 2 hours: 
Standard Deviation of error In nautical mites; 
Prediction Contact Filter 
mean mean mean SPA %Error 


Obs, AOU 


Pred, AO 


Speed in knots. 
Contact Filtered 
mean mean 


7.08 


4.859 


4.547 


4.658 


-2.383 


10 


14.219 


3.041 


3.374 


10.401 


9.202 


8.001 


8.471 


-5,54834 


20 


17.145 


3.017 


3.416 


16.528 


20.859 


15.156 


17.217 


-11,9707 


50 


24.218 


3,023 


2.904 


25.218 


40.585 


24.689 


28.317 


-12,8121 


100 


33.625 


3.068 


2,421 


43.418 


78.444 


43.725 


47.058 


-7,08275 


200 


50.636 


3.065 


2.144 


58.566 


115.538 


58.707 


64.879 


-9,51309 


300 


67.655 


2.897 


1.979 


9.704 


4.805 


4.583 


4.658 


-1,61013 


10 


14.219 


10,066 


8.383 


12.211 


8.83 


7.806 


8.471 


-7.85031 


20 


17,145 


9.972 


7.805 


19.499 


21.68 


16.535 


17,217 


-3.9612 


50 


24.218 


9.964 


7.004 


28.038 


39.535 


25.836 


28,317 


-8.76152 


100 


33,625 


10.025 


6.615 


43.761 


78.011 


42.761 


47.058 


-9.13128 


200 


50.636 


9.958 


6,384 


64.397 


115.971 


63.61 


64.879 


-1.95595 


300 


67.655 


9,975 


6.213 
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22.67 


5.196 


5.242 


4.658 


12.53757 


10 


14.219 


30.061 


24.285 


25.584 


10.066 


9.858 


8.471 


16.37351 


20 


17.145 


29.933 


21.955 


32.774 


22.442 


20.253 


17.217 


17.63373 


50 


24.218 


30.047 


20.111 


47.178 


43.851 


35.221 


28.317 


24.38111 


100 


33.625 


29.957 


19.565 


64.995 


83.895 


56.581 


47.058 


20.23673 


200 


50.636 


29.938 


18.814 


77.334 


119.128 


71.016 


64.879 


9.459147 


300 


67.655 


29.979 


18.895 


Time interval between observations of 1 hours: 










Standard Deviation of error in nautical miles: 








Speed in knots. 


Prediction Contact 


Filter 










Contact 


Filtered 


mean 


mean 


mean 


SPA 


%Error 


Obs. AOU 


Pred. AO 


mean 


mean 


3.607 


2.323 


2.146 


2.296 


-6.5331 


5 


6.429 


3.079 


3.489 


5.215 


4.445 


3.803 


4.159 


-8.55975 


10 


8.15 


2.989 


3.515 


8.437 


10.543 


7.439 


8.435 


-11.8079 


25 


12 


2.96 


3.119 


11.984 


19.878 


11.43 


13.716 


-16.6667 


50 


16.684 


3.032 


2.737 


18.691 


37.993 


18.477 


22.018 


-16.0823 


100 


24.227 


2.984 


2.329 


25.46 


58.34 


25.401 


29.352 


-13.4608 


150 


31.113 


2.991 


2.215 


4.568 


2.393 


2.243 


2.296 


-2.30836 


5 


6.429 


9.988 


8.922 


6.018 


4.461 


3.846 


4.159 


-7.52585 


10 


8.15 


9.904 


8.387 


9.107 


10.411 


7.472 


8.435 


-11.4167 


25 


12 


10.002 


7.7 


13.545 


19.994 


1Z392 


13.716 


-9.65296 


50 


16.684 


10.005 


7.255 


19.93 


37.793 


19.213 


22.018 


-12.7396 


100 


24.227 


10.091 


7.009 


25.759 


54.988 


25.225 


29.352 


-14.0604 


150 


31.113 


9.946 


6.891 


9.34 


2.448 


2.452 


2.296 


6.794425 


5 


6.429 


30.02 


26.103 


10.781 


4.784 


4.551 


4.159 


9.425343 


10 


8.15 


30.055 


24.381 


14.764 


10.961 


9.315 


8.435 


10.43272 


25 


12 


29.998 


22.467 


20.459 


21.364 


15.801 


13.716 


15.20122 


50 


16.684 


29.948 


21.356 


28.395 


39.358 


24.728 


22.018 


12.30811 


100 


24.227 


29.953 


20.68 


36.583 


57.941 


33.572 


29.352 


14.37721 


150 


31.113 


30.002 


20.393 


Time interval between observations of .5 hours: 










Standard Deviation of error in nautical miles: 








Speed in knots. 


Prediction 


Contact 


Filter 










Contact 


Filtered 


mean 


mean 


mean 


SPA 


%Error 


Obs. AOU 


Pred. AO 


mean 


mean 


1.03 


0.488 


0.474 


0.485 


-2.26804 


1 


2.162 


3.046 


3.178 


2.563 


2.203 


1.847 


2.033 


-9.14904 


5 


3.755 


2.988 


3.437 


3.622 


4.095 


3.014 


3.05 


-1.18033 


10 


5.499 


3 


3.298 


5.894 


9.608 


5.522 


6.688 


-17.4342 


25 


8.187 


3.033 


2.875 


8.469 


18.734 


8.256 


10.542 


-21.6847 


50 


11.759 


3.028 


2.58 


11.295 


27.866 


11.156 


13.745 


-18.8359 


75 


14.768 


3.01 


2.435 


1.533 


0.487 


0.475 


0.485 


-2.06186 


1 


2.162 


10.011 


9.607 


2.839 


2.205 


1.874 


2.033 


-7.82095 


5 


3.755 


9.984 


8.905 


4 


4.182 


3.131 


3.5 


-10.5429 


10 


5.199 


9.944 


8.461 


6.342 


9.788 


5.743 


6.688 


-14.1298 


25 


8.187 


10.024 


7,945 
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9.101 


18.558 


8,7 


10.542 


-17.473 


50 


11.759 


10.05 


7.625 


11.972 


27.983 


11.653 


13.745 


-15.2201 


75 


14.768 


9,981 


7.512 


3.513 


0.51 


0.513 


0.485 


5.773196 


1 


2.162 


30.01 


28.592 


4.761 


2.365 


2.207 


2.033 


8,55878 


5 


3.755 


30.038 


26.093 


6.158 


4.456 


3.791 


3.5 


8.314286 


10 


5.199 


30.015 


24.905 


9.329 


10.247 


7.273 


6.688 


8,74701 


25 


8.187 


29.992 


23.28 


13.198 


19.553 


11.421 


10.542 


8.338076 


50 


11.759 


29.652 


22.652 


16.49 


28.111 


14,945 


13.745 


8.730447 


75 


14.768 


30.026 


22.335 


Time interval between observations of .1 hours: 










Standard Deviation of error in nautical miles: 








Speed in knots. 


Prediction Contact 


Filter 










Contact 


Filtered 


mean 


mean 


mean 


SPA 


%Error 


Obs. AOU 


Pred. AO 


mean 


mean 


0.45 


0.43 


0.34 


0.377 


-9.81432 


1 


0.592 


3.002 


3.192 


1.155 


1.921 


1.059 


1.263 


-16.152 


5 


1.504 


3.021 


3.081 


1.689 


3.728 


1.621 


1.995 


-18.7469 


10 


2.22 


3.003 


2.847 


2.084 


5.502 


2.034 


2.573 


-20.9483 


15 


Z78 


2.995 


2.734 


0.495 


0.429 


0.348 


0.377 


-7.69231 


1 


0.592 


10.003 


9.568 


1.241 


1.934 


1.112 


1.263 


-11.9557 


5 


1.504 


10.005 


8.966 


1.761 


3,748 


1.665 


1.995 


-16.5414 


10 


2.22 


10.026 


8.711 


2.221 


5.549 


2.144 


2.573 


-16.6731 


15 


2.78 


10.015 


8.563 


0.778 


0.466 


0.428 


0.377 


13.52785 


1 


0.592 


29.986 


28.414 


1.721 


2.005 


1.368 


1.263 


8.313539 


5 


1.504 


29.998 


26.532 


2.408 


3.832 


2.104 


1.995 


5.463659 


10 


2.22 


30.037 


25.88 


2.947 


5.618 


2.685 


2.573 


4.352895 


15 


2.78 


30.008 


25,47 


Time Interval between observations of .05 hours: 










Standard Deviation of error in nautical miles: 








Speed in knots. 


Prediction Contact 


Filter 










Contact 


Filtered 


mean 


mean 


mean 


SPA 


%Error 


Obs. AOU 


Pred. AO 


mean 


mean 


0.322 


0.4 


0.274 


0.312 


-12.1795 


1 


0.407 


2.971 


3.138 


83 


1.843 


0.795 


0.978 


-18.7117 


5 


1.077 


2.975 


2.919 


0.359 


0.41 


0.291 


0.312 


-6.73077 


1 


0.407 


9.996 


9.549 


0.871 


1.855 


0.82 


0.978 


-16.1554 


5 


1.077 


10.028 


9.011 


0.534 


0.442 


0.37 


0.312 


18.58974 


1 


0.407 


29.997 


28,449 


1.176 


1.908 


1.045 


0.978 


6.850716 


5 


1.077 


30.01 1 


27.085 
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APPENDIX G; MODELS OF ACTUAL ESTIMATED FILTER OPERATION 



This file simulates the basic operation of the ASSET tracker. A contact conducts a random tour at constant speed 
as a contact would in the simulation. Time interval between glimpses is an exponentially distributed variable. 
Statistics are collected to see how well the tracker tracks the target over the course of its path. The number of 
points plotted was adjusted to allow details to be seen. 

j ~ 0..75 runtime ~ 48 glimpse - .1 ^ ))* glimpse mi = 4 v - 10 img - 0 u ”10 mg = u 

OHj ^ - tm^ Nj * until runtime - im^ ,j N2 - l*st( N)*l l = 0..N2 n = 0..N2 44 

^1 = *ln(md(l ))*mi A 2 ^ -ln( md( I ))• mi 4 A 1 A 3 ^ - ln(rad( I ))• mi 4 A 2 A 4 = *ln(md( I ))-ini 4 A 3 A 5 = -ln(md( 1 ))• mi -fA 4 
A 6 = *ln(md( 1 ))Tni 4 A 5 A 7 = -ln(md( I ))*ini 4 A 6 A 8 = -ln(md( I ))*mi 4 A 7 A 9 = -ln(md( I ))* mi 4 A 8 cl - 2 * T* md( I )c 2 = 2 *T*md(l) 



c3 : 2*T*md(l)c4 = 2‘T*md(l)c5 = 2*T*md(l)c6 = 2*T*md(l)c7 = 2*TTnd(l)c8 - 2*T*md(l) c9 = 2*T‘md(l) clO - 2*XTnd(l) 

tc„ = if* tm„ <11 ,el ,if tm„ <12 ,c 2 »i(T tm^ <A 3 ,c 3 »if( tm^ <A 4 ,c 4 ira„ <15 »c 5 ,if tm_ <16 ,c 6 ,i^ tm_ <17 ,c 7 ,if im„ <18 ,c 8 ,ilf im„ <19 , 

n n n * n ’ (^n *’’n ’’j^n “ i” L“ 

randljj = md(l) - md(l) ™o43jj = md(l ) «nd4^ - *^1(1) 






- r r i' 1 ’ 

^ r -2*ln rsndl^ ';*co8t 2* T*r»nd2^ ymg^ = -2*ln(" r»nd3^ i j’sint 2*T*riLnd4^ rcae^ = modi -H- md) j -?;,2*Ti 

vcosi’ [ ic^ I ‘ YdiiQ - 0 XdisQ = 0 



rspdjj - V -^(md(6)- 3) =2*T*md(l) Yspd^^ = vsinr’’ tc^^^ ^ Xspd 



Ydis 

Xdis 



*n-4 Yspd„-A„+Ydis^ 



a = .25 ff - ti — rl - .25-u^ vl„ = rl v2„ = 0 vi,, -- r3 <^2_ = -• 1 -exp' ocA„n 



<„M X5pd„A„+Xd..„ 



Y'POn 

XH.d„ 



^ n j ' tt ^ ^ 



vco» tc. 



n ; ;2 A 

yrng„+YdU„ q2„ = .5 - «xpi -a* : -([ .5-cxp| -a- 2- j I q3^ = .5-^-i 1 - expi -«• 2- i j 

or ' " ' a ' ^ 



Ingn xnig^ +Xdis^ , 

yrsp<>n cspd^- sin' ' rcse„ ^ : j 



xrspdn _ ^ rspd„* 



“jj 

cosi’ * rcsc 1 1 



vl 



n -H 



v3 



n -H 



■ vln ^-^Zn-vZn j <»2nf •v3„ -h|l J-| ; <»3„ ;^-v3„ ■h,3„ ^ v2„ ^e.2„-v3„ - -hi2„^^ 

; vl„ 42- ,^2 „-v 2„ 4[ f • v3„ -tql „ 4rl [ ,^3„ v3„ -h,3„ 4r3 ' - ; ; v2„ 4^2„- v3„ ;• «3„ -h,2„ 



v2„^ = ''2„-HA2 „-v 3„ -«3„-ki2„'Tl- 



[ ''■n +2-<>2n-v2„ + «^2„f -v3„ -h,l„ 4rl - <>3„ ’^-v3„ -kj3„ 4 t3 v 2„ 4t>2„-v3„ ;• <^3„ -Kl2„ 

vIp 42-»2n-v2„ Hr -hjln -frl j-i : A3n'^-v3p -hl3p - V v2p -h»2„-v3p '-03n -hi2p:^ 

''•n +2-<>2 „-v 2„ +' <^2„j^-v3„ -K,l„ 4t1 ’•[ [ <^3„'^-v 3„ -h,3„ 4r3 v2„ v3„ • <^3„ -tq2„'^ 



''7n 

kl : — k2a - — k2b * — 
n rl n r3 n rl 



v3^ 



•^n = -rf ''yo = y™P<^ Xo = ‘««0 Xq r laij, 



mv2 - metn{v2) 
mv3 = me«n(v3 ) 
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i ^ 4i ' 
! vy, 41 



■ •a 4vy , - vy -expi -a*a, ; ; , •, « -^’y, - vy. expi -oc^ i r * -’ 

I ^ 41 •[ ‘•H 41 ^ ^ 41 •, 41 - ''yt‘‘*pi •“ A 

! * X *a 4Vx, - Vy - exp. -a* A. 1 ; [ ' X -a 4 V>l - Vv -exp^ -a-A.'' ’ ^ * i- 

. ^ ^kl, Ing, -A ^ 410.. xr-pd, - VX.-.XP, -orA.,, 



[ r Y *a 4Vy^- Vyj-exp' -a- A, : ' 

Vy,- .xp. -a • A, j 4k2b, 4 , *; Ut, - L ^ ; 



4k3.^,- ynpd, 4 , - Vy.-expr-a-A, ' 



VXj-cxpr-a-Aj" 4k2bj 44* log, 4^ 



r X^*a 4Vxj - Vx^'cxpf -a'A^'"' 



; 4^* -H ' v>4*cxp -a*At 



1 '"‘“'•t 41 


vl, 4-2- 02,-v2, 41 02,j^'v3, 4ql, 


Viou2, ^ ■ 


-! ■ v2, 4t^2,-v3,’-03, 4<i2, 


• 44 ; 


• <^3, ■^•v3, 4q3, 



Parameters for this run: 



Xiou 

Yiou, 



‘t 4 H 



-H 

; V3QOU 



44 I 
Vyiou^ 4 ^ ! 



: X, 4 i-i 1 -cV -a-A."*Vx, 
I t cr ^ ^ 

i Y, 4^*1 I -cxp^-a‘A^■^Vy 

■ o'- ' ^ ^ 

I exp[-orA,;-Vx, 

[ exp[-a-A,|-Vy, 



D - me«n(A] N 2 =49 a =7.07106* a =0.25 u =5 



AOU : Vmeui(vl ) lOU x V<Deui( ViouT) AOU =1.0*7788 lOU =1.197159 

Detennine the difference between the true target position, and the filtered position: 



Fx, = Fy, xY. Ydix, 

the true target position, and the contact position and the true target position, and the lOU-predicted position: 



Cx, - Ing, - Xdi., Cy, = 1«. - Ydix, 



be, = Xiou, - Xdi., 



!y, = Yiou, - Ydix, 



The root-mean-square error is found for the data: 

F2X, =[Fx,f F2y, ^rpy.f C2X, =rcx,f C2y, -[Cy,]2 n*,=[be,f Cy, =[Iy,f 



SFx X ZF2 x SFy = lF2y SCx = ZC2x SCy = ZF2y 



SU = Il2x Sly X II2y 



Fetr 



= iT (SFx) ^ .(SFy)V , 
l*«(Fx) U«(Fy)i 



C.n - f (SCx) ^(SCy) 1. , 



krr 




Fctt = 0.952507 



The mean filtered and contact velocities is found: 



Cerr =1.7404% 



I«fT =1.01*29 



FV, X ^|r Vx,f 4f Vy,1^ I CV, = Jr xfipd, 4J yfipd, mewi(FV) =8.574437 me«i(CV) =10.102023 

mFV = rocAn( FV ) 

.idev(FV) =0.773034 ■idev(CV) =1.641369 mCV = mean(CV) 

The RMS error in filtered position is compared to the RMS error in the 
contact position: 

TR = Cerr-F«rr TR =0.7*79*9 



The difference from the computed error is then computed for 
the current run: 

COMPtrr = AOU - Ferr COMPeir =0.1352*1 
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Filtered track using the actual algorithm, with mean glimpse interval 0.05 hour and Sensor AOU = 5nm: 







At =0.896146 
A2 =1.251034 
A3 =3.394831 
A4 =4.572997 
A5 =6.685208 
A6 =16.686341 
A7 =31.313094 
A8 =31.611555 
A9 =34.12239 



N2 =49 



• ‘ ^ I )))-co« 2“3r*md( I ); 4Xdis^ 



xcsIq = 0 

" .^^*\(-2Mn(md(l )))Mia> 2-T-md(l ); 4Ydis^ 



yciiQ = 0 



xVcj ^ Vx^-exp -a* Aj XT3pd^ ^ * Vx^’cxp' -a* A, 

Vdiffx^ =xVej-V\^ meant Vdiffx) =0.133526 



yVCj ^ Vy^’cxp' -a* At yrspd^ ^ - Vy^-expi -a* Aj j 

Vdiffy^ = yVCj - Vy^ mcan(Vdjffy ) =0.030425 



Filtered track using estimation technique (using full noise values): 




Xditj , liig^ , xctt^ 



Eatx^ - xcsij - XdiXj 
Estyj = ycstj - YdiXj 

Ex^ = ; Eftx^ Eyj = : Esty^ 
SEx = ZEx SEy = lEy 



Ecrr 



. r (SEx) (SEy) .. 

•; last(Ex) MEy), ' 
Eerr =1.028216 



Ferr =0.952507 



vmeao(vl ) =1.087788 
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Filtered track using the actual algorithm, with mean glimpse interval 0.1 hours and Sensor AOU — lOnm: 




Al =2.089507 
A2 =10.470267 
A3 =14 93814 
A4 =17.572047 
A5 =20.489058 
A6 = 22.232939 
A7 =25.150709 
A8 = 25.233842 
A9 =45.284693 



N2 =74 



0 5 

Xdiij .Ing^ 



«"0 



= 0 






= 0 



xcstjj = 'Vl^‘ •. (-2*ln(md(l ) ))* coa[ 2* X* ftid( 1 )] +XdiSj^ 



y®*n " ^’vl^-v(-2Mn(md(l)))*sin[2-T-md(l)] 4YdiSjj 



xVcj = Vx^ cxp a- A, , -Hl3j xrspdj ^ - Vxj*cxp[ -a* Aj ] 1 yVc^ = Vy^^cxpi 'O* A, i -Hc3j ^ yrspd^ ^ ' Vy^-cxpi -a* A, ^ ! 
VdifPx, = xVCj- Vxj mcan(Vdiffx) =0.056692 Vdiffyj = y VCj - Vy^ niean(Vdiffy) =0.013%1 

Filtered track using estimation technique (using full noise values): 
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SEx = lEx SEy = lEy 



_ E,„ . 

Uai(Ex) laii(Ey)j 



Eerr =1.925021 
Fcrr =2.165015 



^;mc*n(vl ) =2.072638 



0 5 

Xdia^ »lxi^ ,xeft^ 
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Filtered track using the actual algorithm, with mean glimpse interval 0.5 hours and Sensor AOU = lOnm: 




150 

Xduj 



=9.779778 
A2 =19.746842 
A3 =23.257026 
A4 =23.336796 
A5 =25.985588 
A6 = 26.130136 
^7 = 27.402486 
AS =28.369044 
A9 =40.739811 



N2 =91 



XCHq 



ycslQ = 0 



xc«n ■ .('2-ln(md(l )))‘co» 2*T*md(l)] 4*XdiSp yc«^ = ^vl^* ^J(-2•ln(^ld(l )))•«« 2* T‘nid(l ) • +Ydis^ 

xVCj = Vx^’cxp -a* A, * Xrtpd^ ^ - VXj'cxpi -a* Aj p yVCj = Vy^’cxp- *a* Aj ^ 4fc3j ^ yrapd^ ^ - Vy^-cxp' -a* Aj 

Vdiffx, =xVcj -Vx, mcan(Vdiffx) =-0.104991 Vdiffy^ = y Vc^ - Vy^ mcan(Vdiffy ) =0.040473 



Filtered track using estimation technique (using full noise values): 




no 



Filtered track using the actual algorithm, with mean glimpse interval 0.5 hours and Sensor AOU = 30nm: 




A1 =3 915512 
A2 =6.91616 
A3 =15 287011 
A4 =20.200269 
A5 =30.204378 
A6 =34 696944 
A7 =35.833538 
A8 =38.376747 
A9 =38,669162 



N2 =96 



XdiSj ,Id«j ,X^ 

xcsi^ = ^Wljj*-^ (-2*ln(md(l )))-coa 2*X‘md(l ) j +XdiSjj yest^ = Jvl^«*/(-2*ln(md(l )))*sin] 2*TTnd(l ) -fYdis^^ 

xVcj = Vxj’cxp -a* A| -HJj • xrspdj ^ - Vx^’cxp -a* A^ ! 1 yVc^ = Vyj«cxpf -a* Aj | -Hc3j ^ yrepd^ ^ - Vy^'cxp -a* Aj ^ 

Vdiffx^ =xVCj-Vx, mcan(Vdiflfx) =-0.056681 Vdifiy^ = y Vc^ - Vy^ mcan(Vdiffy) =-0.145247 

Filtered track using estimation technique (using full noise values): 



ye«0 ■ 
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Filtered track using the actual algorithm, with mean glimpse interval 1 hour and Sensor AOU = lOnm: 




-150 -100 

XdUj ,ingj ,Xj 



A I =5.022727 
A2 =12.437775 
A3 =17.876472 
A4 = 28.662312 
d5 = 42.644194 
= 47.842995 
A7 =49.239743 
A8 =50.274019 
A9 =53.491883 



N2 =45 



= 0 






= 0 



xcst^ = /vl^* •;(-2* in(md( 1 )))*coi[ 2* T* rod(l )] 4Xdis^ ycst^^ = vl '/(-2* ln(md(l )))• linT 2* T* md( I ) j 4Ydi*^ 

xVcj = Vx^-cxp -a* Aj ^ 4k3j ^ xnpdj ^ - Vx^-cxp! -a* A^ J yVc^ = Vy^^cxpi -a* Aj j -Hc3j ^ -f y«pd^ ^ - Vy^-cxp[ -a* Aj 
Vdiffx^ = xVcj - Vx, mcan(Vdiffix) =0.472115 Vdifly^ = y Vcj - Vy^ mcan( Vdiffy ) =-0.66914 



Filtered track using estimation technique (using full noise values): 




112 



Filtered track using the actual algorithm, with mean glimpse interval 1 hour and Sensor AOU = 30nm: 




At =0.155067 
A2 =4 15131 
A3 =9.030112 
M =14.972823 
A5 =19212 
A6 =21.285281 
A7 = 24.284637 
A8 = 33.048225 
A9 =36.179972 



N2 =44 



xertQ = 0 

xcstjj = , vl^‘ ,(-2*ln(md(l )))*cosi 2*T*nid(l ) +XdiSj^ 

xVcj - Vx^-exp -a’Aj xrspd^ ^ - Vx^'cxp. -a* A, yVe^ = Vy^’cxpf -a* A^ ' 4k3^ • yrspd^ ^ - Vy^*cxp 'a•A^ 

Vdiflfx^ = xVcj - Vx^ mean(Vdiffx) =-0.495172 Vdiffy^ = y Ve^ - Vy^ me*n(Vdiffy) =0.01928 



ye«o ' ^ 



Filtered track using estimation technique (using full noise values); 
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Filtered track using the actual algorithm, with mean glimpse interval 1 hour and Sensor AOU = 60nm: 




=0.003175 
A2 =3.366652 
A3 =11.418094 
A4 =14.422879 
A5 =19.245857 
A6 =24.664634 
A7 =26.370376 
A8 =30.333605 
A9 =30.916577 



N2 =54 



xcsijj = •,(-2’ln(md(l )))*coa’ 2*T*md(l )] -hXdiSj^ 

xVc^ - Vxj’cxp -Hc3^ ^ • xrspd^ - Vx^’exp' -a* A^ 

Wdxffx^ ^xVCj-Vx^ cncan(VdifFx) =-0.202094 



xestQ = 0 

^ .^^•\'(-2Mn(fwl(l 2-X-rnd(l )' +Ydis^ 

yVe^ -- Vy^-cxpi -Ht3^ ^ yrspd^ ^ - Vy^-cxp! 

VdiRy^ = y Vc^ - Vy^ metn(Vdifl^) =0.294623 



ye«o = 0 



Filtered track using estimation technique (using full noise values): 




= xeit^ - Xdiij 
= ycstj - YdiSj 

= ' Estx^ ^ Ey^ = ' Estyj 
= ZEx SEy = ZEy 

: a I <SEy) •. g 

^l^last(Ex) laat(Ey)^ 

Ecrr =15.785448 
Ferr =12.430599 

vmean(vl ) =15.165514 
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APPENDIX H: KALMAN GAIN VERSUS SENSOR AOU SIZE 



This appendix shows the relationship between the steady state Kalman gain Ka and the Sensor AOU size. 
The impact of using the the approximations from Appendix A is also illustrated. The data for each case was 
written to a seperate file and the graph alone displayed 



)..25 A ^ .1 


u 


= 1000 


a -- .25 


a - v50 


; rl vIcq : 0 v3cq - r3 

2 




= rl 


v2q =0 v3q = r3 


02 -i 

a 


^* A- -• 2* 1 cxpi a*Ai 

a- « 


: • 


1 - exp[ 


-a*2‘A’’ 1 1 1 1 


2 

02 = ^3- 
a 



r3 = — n = 25-u"^ 
2 - a 



q 2 - — 3 * 1 . -5 • exp[ - a* A j -Ir .5*expj^ -a* 2 * Aj j j 



r vl 
v2 
I v3 



a -H 
n 

a +1 



2 

q3 = .5* — *t 1 - cxpr - a* 2* A I 

a ' 

vl^ 42*<^2*v2g + <^2f *v3g 4ql -Kj3 -fr3 ] - • v2jj *+t^2- v3^ ',• 03 ^2 j 

vl^ -f-2*02*v2j, + 02f-v3jj -Kjl 4 tI j-| [ 03 ]^-v3jj 4q3 4 t 3 ‘-[l v2^ 402-v3„^*03 4q2 f I 

! 1 v2 +02* v3 "'•03 -Kj2 ItI t 3 j 

LL n ^ nj ^ ^ J I 

vl^ +2*02*v2j^ -h 02 ^*v3jj -Kjl +t1 1*1 [ 03 v3jj -ftp -fr3 ^ ^ v2jj -f02*v3jj '*03 -fq2l^ j 

vl^-f-2*02*v2„+r;02f*v3„4ql -frl ;•[[ 03j^ * v3„ 4q3 i -i 1 v2„ +02*v3„ ;*03 4q2 | 

} - _ _ _ _ _ I 

vl^ +2-.<.2-v2^ 4t>2 -V3„ -k)l -trl -v3„ -h,3 [ v2„ -h^2-v3„ ]-«3 -k,2 i 



Using the transition and process noise approximations from Appendix A to simplify the calculations; 

<f>2t a*A-2: A3e = 1 A*[ a* A- 2 ) qle = A j^‘i j A ; q2e = Aj^*[ I -a’A] 

q3c - <7^* A*[ I - a* A J 



4 i : 



vltf^ +2*02 £:*v2Cq -ff 02^]^ *v3Cg -fqlc j*j^[ 03€ j^*v3Cjj 4q3c -ft3 i v2«jj 402c*v3Cjj ,*03e -fq2c | 
vl«^ +-2*02c*v2Cjj -ff 02c]^*v3Cg -fqlc -frl j*' [ 03€ v3c^ -fti3e -fr3 ' -f[ -f02c* v3c^ 03c +ti2c j 

|^[v2*q -f02c*v3Cjj]*03e -fq2«^*rl*r3 j 

vltf^ -f-2*02c*v2e^ -ff 02c J^* v3Cjj -ftjlc -frl j*|^ [ 03c ]^* v3«jj -fq3e -fr3 v2c^ -f02c* v3e^j *03c +q2c ^ j 

vltf^ -f-2*02C: v2e^ -if 02c ]^*v3c^ -fqle -frl 03c ]^*v3Cjj -fq3c ‘ v2Cjj -f02c* v3c^ ^ 03c -fq2c ^ j 

vlCj^ +2* 02 c*v2Cjj -if 02 c]^*v3Cjj -fqlc -frl j*[ [ 03c]^* v3Cjj -fq3c -It 3 • v2Cjj -f02c* v3c^j ^*03€ -fq2c ? J 



vl 

- -r 

n rl n 



r3 



k2b„ = 
n rl 








r3 



k 2 bc„ 



rl 






r3 
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Kalman gain versus sensor AOU in nm for an interarrival time of 0.01 hours: 




Kalman gain versus sensor AOU in nm for an interarrival time of 0. 1 hours: 
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Kalman gain versus sensor AOU in nm for an interarrival time of 1 hour; 




Kalman gain versus sensor AOU in nm for an interarrival time of 2 hours: 
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Kalman gain versus sensor AOU in nm for an interarrival time of 4 hours: 
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